INERTIAL  NAVIGATION  SYSTEM  AIDING  USING  VISION 

THESIS 


James  O.  Quarmyne,  Second  Lieutenant,  USAF 
AFIT-EN  G- 1 3  -M-40 


DEPARTMENT  OF  THE  AIR  FORCE 
AIR  UNIVERSITY 

AIR  FORCE  INSTITUTE  OF  TECHNOLOGY 


Wright-Patterson  Air  Force  Base,  Ohio 


DISTRIBUTION  STATEMENT  A. 

APPROVED  FOR  PUBLIC  RELEASE;  DISTRIBUTION  UNLIMITED. 


The  views  expressed  in  this  thesis  are  those  of  the  author  and  do  not  reflect  the  official 
policy  or  position  of  the  United  States  Air  Force,  the  Department  of  Defense,  or  the 
United  States  Government. 

This  material  is  declared  a  work  of  the  U.S.  Government  and  is  not  subject  to  copyright 
protection  in  the  United  States 


AFIT-EN  G- 1 3  -M-40 


INERTIAL  NAVIGATION  SYSTEM  AIDING  USING  VISION 

THESIS 


Presented  to  the  Faculty 

Department  of  Electrical  and  Computer  Engineering 
Graduate  School  of  Engineering  and  Management 
Air  Force  Insitute  of  Technology 
Air  University 

Air  Education  and  Training  Command 
in  Partial  Fulfillment  of  the  Requirements  for  the 
Degree  of  Master  of  Science  in  Electrical  Engineering 


James  O.  Quarmyne,  B.S.E.E. 
Second  Lieutenant,  USAF 

March  2013 


DISTRIBUTION  STATEMENT  A. 


APPROVED  FOR  PUBLIC  RELEASE;  DISTRIBUTION  UNLIMITED. 


AFIT-EN  G- 1 3  -M-40 


INERTIAL  NAVIGATION  SYSTEM  AIDING  USING  VISION 


James  O.  Quarmyne,  B.S.E.E. 
Second  Lieutenant,  ETSAF 


Approved: 


Meir  Pachter,  PhD  (Chairman) 


Date 


John  F.  Raquet,  PhD  (Committee  Member) 


Date 


Kenneth  A.  Fisher,  PhD  (Committee  Member) 


Date 


Abstract 


The  aiding  of  an  INS  using  measurements  over  time  of  the  line  of  sight  of  ground 
features  as  they  come  into  view  of  an  onboard  camera  is  investigated.  The  objective  is  to 
quantify  the  reduction  in  the  navigation  states’  errors  by  using  bearings-only 
measurements  over  time  of  terrain  features  in  the  aircraft’s  field  of  view.  INS  aiding  is 
achieved  through  the  use  of  a  Kalman  Filter.  The  design  of  the  Kalman  Filter  is  presented 
and  it  is  shown  that  during  a  long  range,  wings  level  cruising  flight  at  constant  velocity 
and  altitude,  a  90%  reduction  in  the  aided  INS -calculated  navigation  state  errors  compared 
to  a  free  INS,  is  possible. 


/  dedicate  this  thesis  to  my  loving  wife,  who  supported  me  every  step  of  the  way. 

I  LOVE  YOU! 


v 


Table  of  Contents 


Page 

Abstract .  iv 

Dedication .  v 

List  of  Figures . viii 

List  of  Tables .  x 

List  of  Symbols .  xi 

List  of  Abbreviations . xiv 

1  Introduction .  1 

1.1  Background .  1 

1.2  Motivation .  2 

1.3  Approach .  3 

2  Literature  Review .  4 

2.1  Introduction .  4 

2.2  Reference  Frames .  4 

2.2.1  Inertial  Reference  Frame .  4 

2.2.2  Earth-Centered,  Earth-Fixed  Inertial  Reference  Frame .  4 

2.2.3  Earth-fixed  Reference  Frame .  5 

2.2.4  Navigation  Reference  Frame .  6 

2.2.5  The  Body  Frame .  7 

2.2.6  Sensor  Frame .  8 

2.3  Coordinate  System  Transformations .  8 

2.4  Inertial  Navigation  .  11 

2.5  Specific  Force  and  Gravity .  11 

2.5.1  Specific  Force .  11 

2.5.2  Gravity .  12 

2.6  INS  Equation .  13 

2.6.1  INS  Equations  for  Common  Frames .  14 

2.6. 1.1  Strapdown  INS  Equation  in  i-frame .  14 

2.6. 1.2  Strapdown  INS  Equation  in  e-frame  .  14 

2.6. 1.3  Strapdown  INS  Equation  in  n-frame  .  14 

2.7  SLAM .  14 

2.8  Camera  Model  .  16 


vi 


2.9  Recent  Research 


16 


3  Methodology  .  19 

3.1  Introduction .  19 

3.2  Development .  19 

3.2.1  Aircraft  Trajectory .  19 

3.2.2  INS  Alignment .  19 

3.2.3  Terrain  Features  Assumptions .  19 

3.3  Approach  and  Model  Description .  20 

3.3.1  Dynamics .  20 

3.3.2  Modeling/Calibrating  the  Free  INS  .  26 

3.3.3  Measurement  Equation .  28 

3.3.4  Synthesis  of  the  Measurement  Sent  to  the  KF  in  Epoch  n .  38 

3.4  Performance  of  Aided  INS .  40 

3.4.1  Initialization  of  the  KF .  42 

3.4.2  Kalman  Filter .  44 

3.4.3  Summary .  56 

4  Simulation  Results  .  58 

4.1  Introduction .  58 

4.2  Simulation .  58 

5  Conclusion  .  64 

Appendix  A:  Simulation  Results .  65 

Appendix  B:  Ground  Feature  Calculations .  73 

Bibliography  .  74 


vii 


List  of  Figures 


Figure  Page 

2. 1  The  I-frame .  5 

2.2  The  i-frame .  6 

2.3  The  e-frame .  7 

2.4  The  n-frame .  8 

2.5  The  b-frame  for  Airplane .  9 

2.6  s-frame  for  INS  Sensor  and  Camera .  10 

2.7  Relationship  Between  g,  G  and  Earth’s  Rate  of  a  Point  Mass .  12 

2.8  Basic  SLAM  Problem .  15 

2.9  Pinhole  Camera  Geometry .  16 

3. 1  Relationship  between  the  body  and  inertial  frame .  21 

3.2  Measurement  Geometry  in  General  Position .  29 

3.3  Showing  the  scenario  set  up . 41 

3.4  INS  Aiding  Using  a  Kalman  Filter .  44 

4. 1  The  development  of  the  KF  predicted  standard  deviation  and  realized  position 

estimates  of  the  unaided  INS  in  the  first  three  measurement  epochs . 60 

4.2  The  development  of  the  KF  predicted  standard  deviation  and  realized  position 

estimates  of  the  aided  INS  for  the  first  three  measurement  epochs . 61 

A.  1  The  development  of  the  KF  predicted  standard  deviation  and  realized  position 

estimate  of  the  aided  INS  during  a  one  hour  flight .  65 

A. 2  The  development  of  the  KF  predicted  standard  deviation  and  realized  position 
estimates  of  the  aided  INS  during  a  one  hour  flight  for  ground  features  arranged 
in  a  straight  line .  66 

A. 3  The  development  of  the  KF  predicted  standard  deviation  and  realized  position 

estimates  in  the  aided  INS  during  a  one  hour  flight  for  staggered  ground  features.  67 


viii 


A.4  The  development  of  velocity  estimates  in  the  aided  INS  during  a  one  hour  flight.  68 

A.5  The  development  of  attitude  estimates  in  the  aided  INS  during  a  one  hour  flight.  69 

A.6  The  calculated  position  of  the  first  ground  feature .  70 

A.7  The  geolocated  second  ground  feature’s  position .  70 

A.  8  A  zoomed  in  view  of  the  development  of  the  KF  predicted  standard  deviation 

of  the  first  ground  feature’s  position  in  first  seven  measurement  epochs . 71 

A.  9  A  zoomed  in  view  of  the  development  of  the  KF  predicted  standard  deviation 

of  the  second  ground  object’s  position  in  first  seven  measurement  epochs.  ...  71 
A.  10  Aircraft  and  ground  objects’  position  estimates  with  KF  predicted  standard 

deviations .  72 


IX 


List  of  Tables 


Table  Page 

4. 1  Standard  Deviations  and  Errors  for  Simulations .  62 

B.l  Ground  Features  on  X-Axis:  X  Position .  73 

B.2  Ground  Features  on  X-Axis:  Y  Position .  73 

B.3  Y  Positions  of  Faterally  Staggered  Ground  Features  on  Both  Sides  of  X-Axis  .  73 


x 


List  of  Symbols 


Symbol  Page 

Cab  DCM  from  b  to  a  Frame .  10 

h  Altitude .  19 

v  Velocity .  19 

x  x  Direction  .  19 

y  y  Direction  .  19 

z  z  Direction .  19 

yp  Inertial  Frame  y  Position  of  Ground  Feature .  20 

i/r  Aircraft  Yaw  Angle .  20 

6  Aircraft  Pitch  Angle .  20 

(p  Aircraft  Roll  Angle .  20 

dx  Navigation  Error  State  Vector .  21 

dP  Position  Error  Vector .  21 

dV  Velocity  Error  Vector .  21 

d*F  Angle  Error  Vector .  21 

du  Disturbance  Vector .  21 

b  Indicates  Body  Frame .  21 

Indicates  Navigation  Frame .  23 

f  Specific  Force  Measured  by  Accelerometer .  23 

a  Total  Aircraft  Acceleration .  23 

g  Specific  Gravity  Vector .  23 

/j-nl  Specific  Force  Measured  by  Longitudinal  Accelerometer .  23 

fyn)  Specific  Force  Measured  by  Lateral  Accelerometer .  23 

f^n)  Specific  Force  Measured  by  Vertical  Accelerometer .  23 

g  Acceleration  of  Gravity .  23 


xi 


a  Longitudinal  Acceleration  of  Aircraft  .  23 

t  Current  Time .  24 

T  Duration  of  a  Measurement  Epoch .  24 

Aa  Continuos-Time  Dynamics  Matrix .  25 

AT  Sampling  Interval .  25 

/  Discrete  Time  Step  Counter .  25 

L  Number  of  Bearing  Measurements  in  an  Epoch .  25 

Aatj  Discrete-Time  Dynamics  Matrix .  25 

xp  Inertial  Frame  x  Position  of  Ground  Feature .  25 

m  Number  of  Unknown  Tracked  Features .  25 

<ra  Standard  Deviation  of  the  Uncertainty  of  the  Accelerometer  Bias .  26 

crg  Standard  Deviation  of  the  Uncertainty  of  the  Gyroscope  Bias .  26 

N  Number  of  Measurement  Epochs .  27 

a  Accelerometer  Bias  Constant .  27 

/ 3  Gyroscope  Bias  Constant .  27 

Xf  x  Coordinate  of  Ground  Feature  on  Camera  Focal  Plane .  28 

yf  y  Coordinate  of  Ground  Feature  on  Camera  Focal  Plane .  28 

/  Focal  Fength  .  28 

zp  Ground  Feature  Elevation .  30 

c  Indicates  Calculated  Value  from  INS .  31 

Indicates  a  Measured  Value .  31 

H(7)  Time  Dependent  Measurement  Matrix .  33 

u  Indicates  Ground  Feature’s  Position  is  Unknown .  33 

k  Indicates  Ground  Feature’s  Position  is  Known .  34 

z  Measurements .  36 

cr  Error  in  One  Pixel .  37 


xii 


x  True  Navigation  State .  37 

5xf  Navigation  State  Error  of  Free  INS .  37 

in)  Current  Measurement  Epoch .  38 

P  Covariance  Matrix  .  42 

6x  Kalman  Filter  Estimates  .  44 

K  Kalman  Gain .  45 

R  Measurement  Uncertainty .  45 


xiii 


List  of  Abbreviations 


Abbreviation  Page 

GPS  Global  Positioning  System .  1 

INS  Inertial  Navigation  Systems .  1 

LO  Low  Observable .  2 

HVT  High  Value  Target .  2 

SLAM  Simultaneous  Localization  and  Mapping .  3 

CG  center  of  gravity .  7 

DCM  Direction  Cosine  Matrix .  10 

KF  Kalman  Filter  .  15 

SS  State  Space .  15 

LOS  Line  Of  Sight  .  17 

UAVs  Unmanned  Aerial  Vehicles  .  20 

RHS  Right  Hand  Side .  32 

LHS  Left  Hand  Side .  32 

SIFT  Scale-Invariant  Feature  Transform .  64 


xiv 


Inertial  Navigation  System  Aiding  Using  Vision 


1  Introduction 


1.1  Background 

Navigation  can  be  defined  as  the  process  of  reading,  and  controlling  the  movement  of 
a  craft  or  vehicle  from  one  place  to  another[l].  Navigation  is  achieved  by  comparing  the 
navigator’s  position  to  known  locations  and  this  can  be  done  in  many  different  ways. 

These  include  but  are  not  limited  to  an  individual  using  a  map,  a  vehicle  using  Global 
Positioning  System  (GPS),  or  an  aircraft  using  an  Inertial  Navigation  Systems  (INS)  in 
combination  with  a  GPS  to  provide  very  precise  navigation. 

An  aircraft  using  an  Inertial  Navigation  Systems  (INS)  aided  by  GPS  measurements 
is  afforded  very  precise  navigation.  GPS  is  very  accurate  but  may  be  occasionally  denied 
due  to  outages.  It  is  therefore  prudent  to  have  workarounds  for  situations  where  the 
precision  available  from  GPS  is  denied.  Aiding  INS  using  the  measurements  of  terrain 
features’  bearings  render  the  integrated  navigation  system  less  dependent  on  GPS.  This  is 
desirable  since  the  vision-aided  INS  will  be  an  autonomous  navigation  system  which  is 
self-contained  and  not  susceptible  to  jamming  and  spoofing.  The  crucial  issues  of 
detection  of  ground  features  in  a  camera’s  field  of  view  and  the  autonomous  tracking  of 
these  features/image  registration,  [5],  [7],  and  [6]  are  not  addressed  in  this  paper.  The 
focus  is  on  gaining  an  understanding  of  the  INS  aiding  action  afforded  by  bearings 
measurements  over  time  of  possibly  unknown  ground  features. 

In  [12]  it  was  shown  using  covariance  analysis  that  the  rate  of  growth  of  position 
uncertainty  is  significantly  reduced  when  the  aircraft  uses  terrain  features  bearing 
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measurements  to  aid  the  INS.  The  same  applies  to  the  uncertainty  in  velocity  and  the 
aircraft’s  Euler  angles. 

This  paper  focuses  on  the  mechanization  of  the  Kalman  Filter  (KF)  for  vision  aided 
INS.  It  is  shown  that  using  the  measurement  over  time  of  the  bearings  of  ground  features 
in  an  aircraft’s  field  of  view,  a  KF  can  significantly  reduce  the  errors  in  the  aircraft’s 
navigation  states  in  a  GPS  denied  environment.  A  cross  country  navigation  scenario  using 
the  concept  of  “bootstrapping”  where  new  ground  features  as  they  come  in  the  camera’s 
field  of  view  are  sequentially  geolocated  and  then  tracked  during  their  residence  in  the 
camera’s  field  of  view,  is  analyzed.  The  Kalman  Filter  is  mechanized  in  the  context  of 
Simultaneous  Focalization  and  Mapping  (SEAM). 

It  is  shown  that  the  synergetic  action  of  the  designed  Kalman  Filter  and  the 
geolocation  algorithm  makes  SEAM  possible  and  using  the  “bootstrapping”  concept  long 
range  flight  which  entails  INS  aiding  using  bearing  measurements  of  unknown  ground 
features  is  feasible-this,  provided  that  the  image  registration  problem  is  solved. 

1.2  Motivation 

Consider  a  Fow  Observable  (FO)  aircraft  carrying  a  FO  munition  on  a  mission  into  a 
territory  to  eliminate  a  High  Value  Target  (HVT).  The  objective  is  to  eliminate  the  target 
with  as  minimal  collateral  damage  as  possible,  so  precision  is  of  utmost  importance.  The 
HVT  is  limited  to  a  small  area  such  that  it  takes  approximately  an  hour  to  fly  from  the 
origin  of  the  military  base.  The  adversary  actively  uses  anti-GPS  technologies,  thus 
denying  the  precision  and  accuracy  of  GPS.  The  aircraft  has  a  navigation  quality  INS  but 
the  duration  of  the  flight  is  long  enough  that  the  errors  produced  by  the  INS  are  too  large 
for  precise  lock  onto  the  HVT.  The  pilot  needs  a  better  navigation  solution,  but  does  not 
want  to  inform  the  enemy  of  the  aircraft’s  presence  by  using  active  navigation  techniques, 
such  as  radar.  The  autonomy  of  the  INS  is  good  for  stealth  but  is  not  enough  to  provide 
precise  navigation  solution  given  the  errors  that  accumulate  over  time. 
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The  proposed  solution  is  to  use  the  bearing  measurements  of  known  and  unknown 
ground  features  to  aid  the  navigation  provided  by  the  INS.  The  aircraft  uses  its  camera  to 
geolocate  ground  features,  track  those  features  to  aid  the  INS,  and  using  that  aided 
estimate  geolocate  new  features  as  the  original  features  leave  the  camera’s  FOV.  This 
aiding  scheme  constrains  the  error  enough  to  obtain  target  solution.  The  on  board 
munition,  with  its  lower  quality  INS,  uses  a  similar  visual  scheme.  It  looks  within  the  area 
given  during  the  mission  briefing  for  the  HVT.  The  munition  impacts  the  HVT  and  the 
aircraft  leaves  the  scene  without  emitting  any  signal  that  will  give  away  its  location. 

1.3  Approach 

This  paper  will  be  structured  such  that  each  chapter  will  begin  with  a  brief 
description  of  topics  that  will  be  forthcoming.  Chapter  2  provides  information  on  the 
various  coordinate  frames  of  reference  that  is  used  when  working  with  an  INS,  the 
transformations  between  the  coordinate  frames,  INS  mechaniztion  equations,  a  brief 
discussion  of  Simultaneous  Localization  and  Mapping  (SLAM)  and  concludes  with  recent 
research  in  the  field.  Chapter  3  shows  the  mathematical  development  of  the  2-D  case, 
including  the  dynamics  and  measurement  model  development,  the  state  space 
representation  and  the  use  of  the  KF  mechanization.  This  information  is  then  extended  to 
look  at  the  3-D  case  for  both  a  horizontal  flight  and  a  vertical  fall.  Chapter  4  looks  at  the 
results  of  the  covariance  analysis.  Finally,  Chapter  5  summarizes  the  key  points  of  the 
paper,  focusing  on  the  impact  the  INS  aiding  scheme  provided. 
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2  Literature  Review 


2.1  Introduction 

This  section  provides  information  on  the  various  coordinate  frames  of  reference  that 
is  used  when  working  with  INS.  Section  2.2  discusses  the  different  coordinate  frames  of 
reference.  Section  2.3  discuses  the  transformations  between  the  various  coordinate 
frames.  Section  2.4  discusses  the  fundamentals  of  inertial  navigation,  including  a  brief 
discussion  of  the  sensors  used  and  how  they  work.  Section  2.6  discusses  the  fundamental 
INS  equation  and  the  INS  equations  for  some  common  frames.  Section  2.7  discusses 
SLAM.  Section  2.8  discusses  the  camera  model  that  will  be  used  as  a  sensor.  Finally, 
Section  2.9  reviews  recent  research  that  contributed  to  this  paper. 

2.2  Reference  Frames 

2.2.1  Inertial  Reference  Frame.  The  Inertial  Reference  Frame  which  is  also 
known  as  the  “true”  inertial  frame  is  denoted  as  the  I-frame.  The  I-frame  is  located  in  the 
sky  and  it  is  in  this  frame  that  Newton’s  laws  apply.  All  reference  frames  used  in  this 
paper  follow  the  right  handed  reference  system.  The  I-frame  is  shown  in  Figure  2.1. 

2.2.2  Earth-Centered,  Earth-Fixed  Inertial  Reference  Frame.  The  Earth-centered, 
Earth-Fixed  (ECEF)  inertial  reference  frame,  as  the  name  imply,  has  its  origin  fixed  to  the 
center  of  the  earth.  It  is  denoted  as  the  i-frame  and  moves  with  the  earth  relative  to  the 
1-frame.  With  the  Earth  modelled  as  an  ellipsoid,  the  axes  of  the  i-frame  are  partially  fixed 
to  the  I-frame  and  are  defined  as  follows: 

•  A',- ax  is  =  along  the  Equator  and  pointing  towards  the  first  star  in  the  Aries 

•  zraxis  =  points  towards  the  North  Pole 

•  y,-axis  =  along  the  Equator,  and  completing  the  right  handed  reference  system 
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The  i-frame  in  the  earth  model  is  shown  in  Figure  2.2. 

2.2.3  Earth-fixed  Reference  Frame.  The  Earth-fixed  reference  frame  has  its  origin 
fixed  to  an  arbitrary  point  on  the  surface  of  the  Earth.  The  axes  of  the  e-frame  are  defined 
as  follows: 

•  .x^-axis  points  to  the  North 

•  ye-axis  points  to  the  east 

•  ze -points  to  the  gravitational  center  of  the  Earth. 

The  e-frame  is  shown  in  Figure  2.3.  Since  the  e-frame  is  fixed  to  an  arbitrary  point  on  the 

surface  of  the  earth,  it  moves  at  the  earth  rate.  The  rotation  between  the  i-frame  and  the 
e-frame  is  denoted  by  0? ,  where  6eie  is  given  by: 

0eie  =  6 0e(t  -  to) 
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Figure  2.2:  The  i-frame 


and  coe  is  the  sidereal  rate  of  the  earth  ~  360 °  /day 

2.2.4  Navigation  Reference  Frame.  The  navigation  frame,  also  known  as  the  local 
level  frame  is  denoted  as  the  n-frame.  The  origin  of  this  reference  frame  is  located  on  a 
plane  which  is  tangential  to  the  surface  of  the  Earth,  where  z  =  0  is  for  the  surface  of  the 
Earth.  The  e-frame  is  fixed  to  the  earth,  but  the  n-frame  is  not.  The  axes  of  the  n-frame  are 
defined  as  follows: 

•  x„-axis  =  points  from  the  origin  to  the  North  pole 

•  y, ,-axis  =  points  to  the  west 

•  z„-axis  =  points  down  away  from  the  center  of  the  Earth 
Figure  2.4  shows  the  n-frame  in  the  Earth  model. 
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Figure  2.3:  The  e-frame 


2.2.5  The  Body  Frame.  The  body  frame,  denoted  as  the  b-frame  is  rigidly  attached 
to  the  body  of  a  vehicle  (airplane,  car,  ship,  etc.).  The  origin  is  located  somewhere  on  the 
body,  either  the  center  of  gravity  (CG)  or  something  measurable.  The  axes  do  not  change 

with  changes  to  an  aircraft’s  trajectory  or  orientation  of  a  car.  Due  to  the  fact  that  an 

aircraft  loses  fuel  in  the  cause  of  flight  and  the  center  of  gravity  will  consequently  change, 
the  origin  of  the  body  frame  will  be  fixed  to  the  origin  of  the  camera  in  this  paper.  The 
body  axes  are  defined  as  follows: 

•  vy-axis  =  points  out  of  the  nose  of  the  aircraft 

•  y /,-axis  =  points  out  of  the  left  wing  of  the  aircraft 

•  Zb- axis  =  points  out  of  the  top  of  the  aircraft 
Figure  2.5  shows  the  body  frame  for  an  airplane. 
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Figure  2.4:  The  n-frame 


2.2.6  Sensor  Frame.  The  sensor  frame,  denoted  as  the  s-frame  is  defined  by  the 
designer  using  the  right  handed  reference  system.  It  is  completely  up  to  the  designer  to 
define  the  origin  and  axes  of  the  s-frame.  Figure  2.6  shows  two  sensors  (INS  and  camera) 
with  their  respective  s-frames  in  relation  to  the  b-frame  of  an  object  whose  origin  is  at  the 
CG.  Again,  for  reasons  discussed  in  sub-section  2.2.5  the  origin  of  the  body  frame  will  be 
co-located  with  the  origin  of  the  camera  frame  in  this  paper. 

2.3  Coordinate  System  Transformations 

In  INS  computations,  it  is  often  convenient  to  convert  the  different  frames  to  a  single 
frame  (often  the  navigation  frame)  for  easy  calculations.  In  order  to  transform  points  and 
vectors  from  one  frame  to  the  other,  there  is  the  need  to  perform  either  translation, 
rotation,  or  both.  Translation  is  an  n  x  1  vector  that  relates  the  origins  of  two  frames  of 
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Figure  2.5:  The  b-frame  for  Airplane 

interest.  The  translation  of  a  point  P,  from  a-frame  to  b-frame  in  the  a-frame  is  denoted  by 
Pa,.  Rotations  on  the  other  hand  are  defined  with  respect  to  the  orthogonal  right-handed 
axis  set.  Rotation  of  a  set  of  axes  in  one  frame  to  another  frame  can  be  done  in  one  of 
three  ways,  namely: 

1.  Euler  angles  (cp,  8,  ip):  This  is  a  transformation  of  one  frame  to  another  by  three 
successive  rotations  about  three  different  axes  taken  in  turn  [13].  It  is  worth  noting 
that  the  order  of  rotation  matters;  rotation  in  the  order  (cp, 8,  ip)  is  different  from 
rotation  in  the  order  ( 8 ,  if/,  (p). 

2.  Quaternions  (4x1  vector):  The  quaternion  attitude  representation  allows  a 
transformation  from  one  coordinate  frame  to  another  to  be  effected  by  a  single 
rotation  about  a  vector  defined  in  the  reference  frame.  The  quaternion  is  a 
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Camera 


Figure  2.6:  s-frame  for  INS  Sensor  and  Camera 


four-element  vector  representation,  the  elements  of  which  are  functions  of  the 
orientation  of  this  vector  and  the  magnitude  of  the  rotation. 


3.  Direction  Cosine  Matrix  (DCM)  (3x3  matrix):  The  Direction  Cosine  Matrix 
(DCM),  is  a  3  x  3  matrix,  the  columns  of  which  represent  unit  vectors  in  body  axes 
projected  along  the  reference  axes.  The  DCM  from  a  b-frame  to  an  a-frame  is 
denoted  by  C?,  which  is  written  as  follows: 


Cu 

c  12 

C13 

c2I 

c22 

C23 

C3 1 

C32 

C33 

(2.1) 


Where  the  element  in  the  zth  row  and  the  jth  column  represents  the  cosine  of  the  angle 
between  the  /-axis  of  the  a-frame  and  the  /-axis  of  the  b-frame. 
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This  paper  uses  DCM  for  coordinate  system  transformations.  For  right-handed, 
orthonormal  reference  frames,  some  DCM  rules  are  as  follows: 

Det(Cl)  =  \q\  =1 
(Cabrx  =  ( cab)T  =  cba 

/~^a  _  s-ia  /~<b 

2.4  Inertial  Navigation 

This  section  provides  an  overview  of  the  basic  principles  of  inertial  systems.  To 
navigate,  knowledge  of  the  measurements  of  specific  force  and  angular  rates  are  required. 
These  measurements  are  provided  by  an  INS,  which  consists  of  accelerometers  and  gyros. 
Accelerometers  measure  specific  force,  while  gyros  measure  angular  rate.  An  INS  is  a 
self-contained  and  nonjammable  navigation  instrument  that  provides  redundancy  for  radio 
navigation  systems  that  can  experience  interference  or  be  jammed;  however,  an  INS  does 
suffer  from  drift,  the  unbounded  growth  of  errors  over  time.  Even,  with  perfect  alignment, 
accelerometer  biases  and  gyro  drift  causes  the  errors  in  INS  to  grow  over  time  [10]. 

2.5  Specific  Force  and  Gravity 

2.5.1  Specific  Force.  From  Newton’s  Second  Law  of  Motion,  the  force,  Fj  acting 
on  a  body  of  mass  m,  moving  with  an  acceleration  of  pi  in  the  inertial  frame  is  given  by: 

Fi  =  mpf  (2.2) 

Accelerometers,  as  was  mentioned  earlier,  measure  specific  force,//,  and  it  is  defined 
as  the  inertial  force,  F /,  per  unit  mass  m,  required  to  produce  the  acceleration  pi.  This 
relationship  is  given  by  Equation  2.3. 

f,=  —  «fi  (2-3) 

m 

Examples  of  inertial  forces  include  spring  force,  friction,  lift,  thrust,  and  support. 
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Greenwich 

meridian 


latitude 


Figure  2.7 :  Relationship  Between  g,  G  and  Earth’s  Rate  of  a  Point  Mass 


2.5.2  Gravity.  The  acceleration  pj,  acting  on  a  particle  in  a  gravitational  field  is 
given  by  the  fundamental  equation  of  inertial  navigation  as: 

Pi  =  fi  +  Gt  (2.4) 

Gravity  g,  is  then  defined  by  equation  2.5  below. 

g  =  G(p )  -  Clie0.iep  (2.5) 

where  G  is  the  Earth’s  gravitational  force  acting  on  the  particle  at  position  p  and  Q/e  is  the 
centrifugal  force  pulling  outward  due  to  the  rotation  of  the  earth.  It  is  worth  noting  that 
gravity  is  not  an  inertial  force.  This  relationship  is  as  depicted  in  Figure  2.7. 

This  effect  is  not  very  significant,  since  the  centrifugal  force  is  only  a  fraction  of  the 
gravitational  force: 

\\nAp\\*^:\\G(p)\\  (2.6) 
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2.6  INS  Equation 


Consider  relating  the  time  derivative  of  vectors  in  rotating  reference  frames.  From 
vector  addition: 


a0p 


yjd  VCl 

raob0  rb0p 


(2.7) 


where  r“  „  and  ra  ,  and  it  is  the  desire  to  find  r?  in  the  /?-framc: 

aoP  aobo  bop 


D  _  r'b  a  _  piO/  a  _  a  \  _  r'O  a  0 

lbop  ^albop  ^a\Laop  1  aobo*  aLaop  '  1  boat) 


-ibs^a 


b^ja 


(2.8) 


For  short-hand,  write  as 


nb  t~<bna  ,  „b 

P  =  E„p  +  rba 


(2.9) 


Taking  the  time  derivative  of  p/(  yields: 


£r„\  a  _  d  i  r^b  d 

dt 


P°  =v°  =  -  C*  pfl  +  C“-  pfl  + 


dt'  a 

''br^a  na 


‘dt 


dt 


ba 


CW'  +  C^pj  +  |(rL 


v°  =  -  rL|  +  C*(n>°  +  ^) 


(2.10) 

(2.11) 

(2.12) 


where  4(r bba)  accounts  for  the  relative  velocity  betwwen  the  a- frame  and  Z?-frame, 
CaWbP1'  is  the  instantaneous  velocity  of  p  relative  to  the  b- frame  due  to  the  relative 
rotation  of  the  a-frame,  and  Cbava  is  the  instantaneous  velocity  of  p  in  the  a- frame 
transformed  into  /?- frame.  Taking  another  time  derivative  of  Eq.  2.12  results  in: 


A  _  ^rb  ,  l 
dt\  dt 2  ba  dt 


cba  (n^p°  +  va) 


,  dC«(™na  ,  ^  ,  rbd(nabPa  +  V°) 

=  rto  +  — (Jl06p  +  v)  +  C„ - - - 

=  fL  +  (c*niVn>“  +  V)  +  c^p"  +  n>°)  +  c>“ 


a6  =  4  +  cjKn^ni  +  n:;s)p“  +  2n;>-  +  a-] 


(2.13) 

(2.14) 

(2.15) 

(2.16) 


Eq.  2.16  represents  the  fundamental  relationship  for  an  INS. 
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2.6.1  INS  Equations  for  Common  Frames.  The  previous  section  dealt  with  INS 
equations  for  some  arbitrary  frames.  This  section  will  briefly  provide  the  strapdown  INS 
equations  for  the  following  frames: 


2.6. 1.1  Strapdown  INS  Equation  in  i-frame.  In  the  i-frame,  Eq.  2.16  reduces 


to 


d2Pi 

dt2 


-  C'hfb  +  gi 


(2.17) 


2.6. 1.2  Strapdown  INS  Equation  in  e-frame.  In  the  e-frame,  Eq.  2.16  reduces 


to 


d2pe 

dt2 


Cebfb  +  ge-2Q?ieve 


(2.18) 


2.6. 1.3  Strapdown  INS  Equation  in  n-frame.  In  the  n-frame,  Eq.  2.16  reduces 


to 


d2pn 

dt2 


Clfb  +  gn  ~  (2X2>„  +  Q"  )v„ 


(2.19) 


2.7  SLAM 


Maps  are  needed  to  depict  an  environment  for  planning  and  navigation.  They  may  or 
may  not  be  readily  available  depending  on  the  environment  of  interest.  In  the  case  that 
they  are  not  readily  available  (due  to  topographical  changes  or  an  unfamiliar  indoor 
environment),  the  techniques  of  SLAM  come  in  very  handy.  SLAM  is  a  process  by  which 
a  mobile  robot  or  an  autonomous  vehicle  can  build  a  map  of  an  environment  and  at  the 
same  time  use  this  map  to  deduce  its  location  [2].  The  essential  SLAM  problem  is  shown 
in  Figure  2.8  [2] 

To  understand  SLAM,  consider  a  mobile  robot  having  an  onboard  sensor,  a  camera 
for  example,  moving  through  an  unknown  environment  with  no  a  priori  information 
about  the  environment.  The  robot  probabilistically  estimates  its  own  position  and  uses  the 
onboard  camera  to  estimate  the  position  of  unknown  landmarks.  It  is  a  recursive  process 
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Figure  2.8:  Basic  SLAM  Problem.  The  essential  problem  of  SLAM  requires  the 
simultaneous  estimation  of  both  robot  or  autonomous  vehicle  and  landmark  positions. 
Neither  position  is  truly  known[2]. 


where  the  robot  uses  its  position  to  estimate  the  position  of  unknown  ground  features  and 
then  uses  the  the  estimates  of  the  ground  features  to  estimate  its  own  position.  This 
recursive  process  is  typically  achieved  through  the  use  of  a  Kalman  Filter  (KF). 

With  the  introduction  of  each  additional  unknown  ground  feature,  the  states  used  in 
the  State  Space  (SS)  equation  of  the  KF  increases  depending  on  the  type  information 
required  by  the  KF  for  its  estimation.  In  this  paper,  the  x  and  y  positions  of  stationary 
ground  features  are  the  informtation  of  interest  so  with  the  addition  of  an  unknown  ground 
feature  whose  position  is  to  be  estimated,  the  states  in  the  SS  of  the  KF  increase  by  two. 
This  is  because  stationary  ground  features  are  at  zero  elevation,  so  only  the  x  and  y 
positions  of  the  ground  features  are  considered.  Likewise,  with  the  drop  of  ground  feature 
by  the  sensor  (camera)  because  it  is  no  longer  in  its  FOV  the  states  in  the  SS  of  the  KF 
decrease  by  two. 


15 


2.8  Camera  Model 


In  this  paper,  the  camera  will  be  modelled  as  the  basic  pinhole  camera  where  it  is 
assumed  that  there  are  no  camera  distortions.  Camera  caliberation  will  therefore  not  be 
considered.  The  basic  pinhole  camera  is  shown  in  Figure  2.9.  The  focal  length  of  the 


Figure  2.9:  Pinhole  Camera  Geometry.  C  is  the  camera  centerand  p  the  principal  point. 
The  camera  center  is  placed  at  the  coordinate  origin  [4]. 


camera  is  /.  Features  are  considered  as  points  which  are  projected  in  space  onto  an  image 
plane.  A  point  in  space  with  coordinates  X  =  (X,  Y,  Z)T  is  projected  onto  the  image  plane. 
From  the  geometry,  it  can  be  computed  by  similar  triangles  that  ( X ,  Y,  Z)T  maps  onto  the 
point  (JX/Z,fY/Z,f)r  on  the  image  plane[4].  Ignoring  the  final  image  coordinate,  it  can  be 
seen  that 

(X,  Y  Z)T  (fX/Z,fY/Z)r  (2.20) 


2.9  Recent  Research 

In  [10],  Pachter  et.  al  researched  the  idea  of  using  bearings-only  measurements  for 
aiding  INS.  This  was  a  theoretical  work,  where  no  simulations  or  empirical  data  were  used 
to  substantiate  the  theory.  The  significance  of  this  research  was  the  development  of  the 
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mathematics  for  observing  the  Line  Of  Sight  (LOS)  angle  measurements  to  a  ground 
feature  over  time  and  using  that  information  to  update  an  INS.  The  update  was  theorized 
to  constraining  the  unbounded  errors  developed  by  an  INS  if  it  were  allowed  to  operate 
freely. 

In  [3],  Giebner  aided  an  aircraft  INS  using  visual  measurements  when  the  aircraft 
flies  in  a  circular  orbit  around  a  several  ground  features.  A  KF  was  used  to  achieve  the 
aiding  and  this  was  done  both  in  simulation  and  in  an  actual  test  environment.  It  was 
found  that  the  uncertainty  in  the  aircraft’s  position  after  six  minutes  of  flight  time,  was 
reduced  from  350  meters,  in  the  unaided  INS  case,  down  to  50  meters,  in  the  aided  INS 
case,  when  the  visual  measurements  were  combined  with  barometric  altimeter  readings. 

In  [9],  Pachter  and  Mutlu  explored  the  observability  of  a  vision-aided  INS.  The 
bearing  measurements  used  were  time  dependent  because  the  position  of  the  ground 
feature(s)  being  tracked  by  the  aircraft  changed  with  time.  The  time  dependent 
measurement  matrix  prompted  the  use  of  observability  Grammian.  It  was  determined  that 
using  the  bearings-only  visual  measurements  of  a  single  ground  feature  to  aid  the  INS,  the 
observability  Grammian  was  rank  deficient,  making  the  INS  aiding  action  incomplete.  In 
order  for  full  rank  observability  Grammian  and  thereby  have  complete  INS  aiding  action, 
a  second  ground  feature  had  to  be  simultaneously  tracked.  Complete  INS  aiding  action 
means  all  of  the  navigation  states  receive  some  improvement  from  the  measurement  when 
compared  to  the  unaided  INS. 

In  [2],  Durrant-Whyte  and  Bailey  provided  the  origins  of  SLAM.  It  was  shown  how 
various  filter  methods  can  be  used  to  implement  SLAM  using  the  limited  information  in  a 
robot’s  environment.  The  uncertainty  of  detected  features,  as  well  as  the  navigation 
estimate,  was  shown  to  be  dependent  on  the  number  of  measurements  taken.  The  more 
measurements  that  were  taken,  the  less  the  uncertainty.  In  a  scenario  where  a  robot  was 
remotely  piloted  through  an  indoor  environment,  a  pilot  with  no  visual  access  of  the  robot, 


17 


the  robot  autonomously  returned  to  its  starting  point  using  map  that  it  build  up  during  the 
navigation  process. 

In  [14],  Veth  looked  at  the  fusion  of  imaging  and  inertial  sensors  for  navigation.  A 
statistical  feature  projection  technique  was  developed  which  utilizes  inertial 
measurements  to  predict  vectors  in  the  feature  space  between  images.  The  feature  matches 
and  inertial  measurements  were  used  to  estimate  the  navigation  trajectory  on-line  using  an 
extended  Kalman  filter. 

This  paper  is  a  continuation  of  [12].  In  [12]  it  was  shown  using  covariance  analysis 
that  the  rate  of  growth  of  position  uncertainty  is  significantly  reduced  when  the  aircraft 
uses  terrain  features  bearing  measurements  to  aid  the  INS.  The  same  applies  to  the 
uncertainty  in  velocity  and  the  aircraft’s  Euler  angles.  The  method  used  for  geolocating 
new  ground  features  were  crude,  where  only  the  aircraft  position  was  used  in  the 
geolocation  process.  This  paper  will  look  at  a  more  accurate  method  (include  the  other 
navigation  states  in  the  geolocation  of  ground  features)  of  calculating  the  covariances  and 
implement  a  KF  in  a  simulation  analysis  to  substantiate  the  theory. 
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3  Methodology 


3.1  Introduction 

This  chapter  is  broken  up  into  three  main  sections.  Section  3.2  discusses  the 
navigation  scenario  that  is  considered  for  the  dynamics  model  development.  Section  3.3 
discusses  the  approach  that  is  taking  in  calibrating  the  unaided  INS  with  small  angle 
assumptions,  measurement  model  development,  the  measurement  equations  that  are  sent 
to  the  KF,  and  how  the  calculations  are  accomplished.  Section  3.4  discusses  the 
performance  of  the  aided  INS,  which  includes  the  KF  and  its  initialization  at  the 
beginning  of  each  epoch. 

3.2  Development 

The  navigation  scenario  is  as  follows: 

3.2.1  Aircraft  Trajectory.  The  aircraft  is  flying  wings-level  at  a  constant  altitude  h. 
The  ground  speed  of  the  aircraft  is  constant  and  the  aircraft  flies  in  the  positive  xn 
direction. 

3.2.2  INS  Alignment.  The  initial  INS  alignment  is  considered  to  be  “perfect”. 

That  is,  at  the  start  of  the  flight  at  altitude  h  and  velocity  v  in  the  positive  xn  direction,  the 
exact  aircraft’s  position,  velocity,  and  attitude  are  known  with  very  small  errors.  The 
emphasis  is  on  the  contribution  of  the  inertial  instruments’  errors  to  the  INS  navigation 
state  errors.  In  this  respect  it  is  assumed  that  the  x,  y,  and  z  accelerometers  are  of  the  same 
quality;  also  the  x,  y,  and  z  gyroscopes  are  of  the  same  quality  and  the  instruments’ 
measurement  error  is  modeled  as  a  random  bias. 

3.2.3  Terrain  Features  Assumptions.  At  all  time  two  ground  features  need  to  be 
tracked  for  observability  [9].  Thus,  it  is  assumed  that  the  position  of  the  first  two  ground 
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features  to  come  into  the  aircraft  camera’s  field  of  view  are  known.  The  position  of 
subsequent  features  are  not  known  but  these  are  geolocated  as  they  come  into  the  camera’s 
field  of  view  a  la  SLAM.  Bearing  measurements  to  these  newly  acquired  features  are 
subsequently  taken,  hence  the  “bootstrapping”  concept.  Obviously,  for  vision-aided 
navigation  to  be  possible,  one  cannot  fly  over  featureless  terrain  and  the  features  need  to 
be  more  or  less  regularly  spaced.  Hence,  without  loss  of  generality  it  is  assumed  that  the 
features  are  nominally  equally  spaced  in  the  positive  xn  direction  and  are  at  zero,  a.k.a. 
known,  elevation.  Two  scenarios  are  considered.  First  the  ground  features  are  arranged  in 
a  perfect  straight  line  along  the  aircraft’s  trajectory,  and  second,  the  ground  features  are 
laterally  staggered  yp  meters  about  the  x„  axis.  Thus,  in  the  first  navigation  scenario,  yp  is 
zero  meters  and  all  the  terrain  features  are  on  the  xn  axis.  The  Earth  is  assumed  flat  and 
nonrotating.  This  assumption  is  reasonable  considering  the  relatively  short  range  and/or 
the  tactical  grade  specification  of  the  gyros  and  accelerometers  of  small  Unmanned  Aerial 
Vehicles  (UAVs)  for  which  this  autonomous  navigation  system  is  being  developed. 

Kalman  filtering  in  a  SLAM  scenario  where  the  aircraft  uses  inertial  navigation  is 
considered.  Our  novel  approach  to  SLAM  is  rooted  in  the  theory  of  inertial  navigation,  as 
opposed  to  robotics  or  computer  science. 

3.3  Approach  and  Model  Description 

3.3.1  Dynamics.  The  navigation  n-frame  is  the  Earth  fixed  “inertial”  (x n,yn,z„) 
frame.  The  aircraft’s  body  axes  are  ( xt,yb,Zb )•  The  aircraft’s  and  camera’s  position  in  the 
navigation  frame  is  (x,y,z),  with  ip.  0,  and  (/)  as  its  Euler  angles.  A  strapdown  [13]  INS 
arrangement  is  considered.  When  flying  over  a  non-rotating  and  flat  Earth  as  shown  in 
Figure  3.1,  the  INS  error  dynamics  can  be  considerably  simplified  [11],  [12],  [13].  The 
simplified  dynamics  of  the  INS  errors  in  state  space  notation,  also  known  as  the  error 
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equations,  are  dx  =  A dx  +  rdu,  where  the  navigation  error  state  vector  Sx  given  by 


Sx  =  [  SP  dV  dY  f  (3.1) 

are  the  errors  in  the  navigation  state’s  position  dP,  velocity  dV,  and  angles  dY,  and  the 
disturbances  du  are  the  three  accelerometers’  and  the  three  rate  gyroscopes’  random  biases 

£u  =  [  d/b  d/yb  d./:b  6cobx  6ojb  \'  (3.2) 

The  superscript  b  indicates  that  the  body  frame  of  reference  is  being  used.  The  errors  in 


Figure  3.1:  Level  Flight  at  Constant  Altitude  Along  the  x„-axis 


the  angles,  dY,  are  given  by 


and 


dY  =  -dCbCb  (3.3) 

dY  =  dYx  (3.4) 
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where  Cb  is  the  DCM  and  ci'F  is  the  skew  symmetric  matrix  formed  from  the  angle  errors 
vector  <5VF  according  to  Eq.  (3.4). 

For  small  Euler  angles  p,  6,  p,  the  DCM 

1  -p  6 

C^(p,6,p)  =  p  1  -p  (3.5) 

-6  p  1 

and  therefore  its  perturbation 

0  -Sp  86 

SCI  -  Sp  0  —8p  (3-6) 

-86  Sp  0 

For  constant  altitude  flight  in  the  direction  of  the  xn  axis,  the  nominal  Cb  =  I3.  Thus,  using 
Eq.  (3.3)  the  following  is  calculated 

0  8p  -86 

^  =  -8p  0  8p  (3.7 ) 

86  -Sp  0 

and  since  =  d'vFx  the  errors  in  the  aircraft’s  Euler  angles  are  recovered 

W  =  [  -Sp  -86  -8p  f  (3.8) 

Hence,  the  navigation  state’s  error  vector  is 

dx  =  [  Sx  8y  Sz  8vx  8vy  8vz  -8p  -86  -Sp  \T  (3.9) 


and  the  INS  error  state  equations  are 


03x3 

1-3x3 

03x3 

03x3 

03x3 

Sx  = 

03x3 

03x3 

F(n) 
r  3x3 

8x  + 

r*b 

03x3 

03x3 

03x3 

03x3 

03x3 

-cb 

'-'n 

(3.10) 
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where  F,")  =  f(n,x  is  the  skew  symmetric  matrix  form  of  the  specific  force  vector  f(n).  The 
superscript <n>  indicates  that  the  inertial  navigation  frame  of  reference  is  being  used.  The 
specific  force  measured  by  the  accelerometer  f ,  total  aircraft  acceleration  a,  and  the 
specific  gravity  vector  g  are  related  according  to  Eq.  2.4  by  f  =  a  -  g,  that  is, 

f(n)  =  a(n)  -  g(n).  During  wings  level  flight 

(  \  (  \ 

a  0 

a(n)  =  0  ,  g(n)=  0 

,  0  i  i  -8  , 

Therefore  the  nominal  specific  force  components  during  constant  altitude,  wings  level 
flight  are  jT'-  a,  /v(n,=  0  and  /z(n)=  g,  where  g  is  the  acceleration  of  gravity  and  a  is  the 
longitudinal  acceleration  of  the  aircraft.  Thus, 

[  A“  1  [  «  1 


f(n)  =  /v(n)  =  0 


(3.11) 


Eqs.  (3.10)  and  (3.11)  represent  the  dynamics  of  the  navigation  state’s  error,  (fiP,  8\,  fi*P), 
under  the  assumption  that  the  Earth  is  flat  and  non-rotating.  The  meaning  of  the  angular 
errors’  vector  <5VF.  that  is,  its  relationship  to  the  Euler  angles’  errors,  is  determined  by  the 
aircraft’s  trajectory,  that  is,  the  nominal  DCM  C7.  In  the  special  case  of  wings  level  flight 
when  the  body  and  navigation  frames  are  aligned  as  shown  in  Figure  3.1,  the  angular 
errors  are  the  Euler  angles.  However,  having  negative  angle  error  states  is  unorthodox.  In 
order  for  the  navigation  state  error  to  be 


fix  -  [  8x  8y  8z  Svx  6vy  8vz  6(f)  88  Si//  ] 
the  dynamics  Eq.  (3.10)  is  modified  as  follows 


(3.12) 


03x3 

1-3x3 

03x3 

03x3 

03x3 

8x  = 

03x3 

03x3 

r  3x3 

8x  + 

03x3 

03x3 

03x3 

03x3 

03x3 

r^b 

(3.13) 
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and  for  “perfect”  INS  alignment  with  very  small  uncertainties, 


where 


(  <5P(0)  } 


<5x(0)  = 


<5V(0) 


5¥(0) 


'9x1 


(dP'b)(0),dP^(0),dPib,(0)  ~  iV(03xl,  1  X  10-6I3) 
(hV^(0),hV;b)(0),hV'b,(0)  ~  iV(03xl,  1  x  10-16I3) 
(5^(0), d^b,(0),  5^(0)  ~  iV(03xi,  1  x  10-8I3) 


Since  this  is  wings  level,  constant  altitude  flight,  in  the  direction  of  the  xn  axis,  the 
nominal,  true  navigation  variables  are 

x  =  Xo  +  vxt  +  \ at 2,  y  =  0,  z  =  h,  <f>  =  9  =  if/  =  0.  These  variables  are 
non-dimensionalized  as  follows 


x 

h’ 

Vx 

9 

V 


r 


y 

h’ 

Vx 

9 

V 


Sfx- 

Sfx 

4  — , 

Sfy  ~ 

SJy 

~ ^  9 

6fz 

8 

8 

8(o° 

8a>b 

8cohx- 

4  h - , 

ScJy- 

4  h  \ 

z 

h’ 

Vz 

9 

V 

SJz 

9 

8 

8cab 

h—. 


v 


V 


T- 

h' 


where  t  is  the  current  time,  and  T  is  the  length  of  a  measurement  epoch. 
The  non-dimensional  parameters  are 

ha 


a  hg 

8  =  ~ 
v 2 


and 


A 

a  = 


During  cruise,  a  =  0.  If,  for  example, 

h  =  1000[m],  v  =  100 


m 


L  sec 


8=  10 


m 


L  secz 
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the  non-dimensional  parameter  g  =  1.  Since  the  ground  features  are  spaced  1  [km]  apart, 
the  duration  of  a  nondimensional  measurement  epoch  T  =  1 . 

It  is  assumed  that  the  sensor  errors  are  constant,  albeit  random  biases  that  are 
Gaussian  distributed.  This  allows  the  state  error  vector  to  be  augmented  with  the 
disturbance  vector  du;  the  augmented  state  is 

/  \ 

dx 

5xa  =  ...  (3.14) 

du 

v  '15x1 

and  the  dynamics  matrix  is  augmented  by  the  T  matrix,  as  shown 

a  r 

Aa  =  (3.15) 

06x9  06x6 

L  -45x15 

One  obtains  a  dynamic  system  in  “free  fall”.  When  converted  to  discrete  time, 

Aa— >  Aad  =  eAaAT,  where  A T  is  the  sampling  interval.  The  augmented  discrete  time  state 
dynamics  become 

dxa(/  +  1)  =  Aaddxa(/),  /  =  0, . . . ,  L  -  1  (3.16) 


where  /  is  the  discrete  time  step  counter  and  L  is  the  total  time  during  a  measurement 
epoch  during  which  the  two  ground  features  are  being  tracked.  The  non-dimensional  time 
step  is  AT  ATjr  The  discrete-time  dynamics  matrix  Aad  can  be  analytically 

derived. 

This  dynamics  equation  applies  as  long  as  the  ground  objects’  positions  (xp,yp)  are 
known.  Assuming  the  ground  objects  are  stationary,  but  their  position  is  not  known,  two 
additional  states,  the  x  and  y  horizontal  coordinates  of  the  tracked  ground  objects,  must  be 
added  to  the  navigation  state  for  each  tracked  ground  object  whose  position  will  be 
estimated  on  the  fly.  If  the  number  of  unknown  ground  features  being  tracked  is  m,  then 
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the  augmented  navigation  state  is 


f  dxa  ) 


6xa  :  = 


Sxp  i 


(3.17) 


and 


Aafl  .  — 


0 


l 

pm 

Aad 

®15x2  m 

bmx  15 

I-2rax2m 

(15+2m)xl 


J(15+2m)x(15+2m) 


(3.18) 


If,  for  example,  one  unknown  ground  feature  is  being  tracked,  as  is  the  case  during  the 
second  measurement  epoch,  then  the  dimension  of  the  augmented  navigation  state’s  error 
is  17.  Two  unknown  ground  features  are  being  tracked  during  the  measurement  epoch 
n  >  3,  whereupon  the  dimension  of  the  navigation  state’s  error  is  19.  On  one  hand,  state 
augmentation  reduces  the  degree  of  observability,  which  decreases  the  strength  of  INS 
aiding  action.  On  the  other  hand,  however,  the  inclusion  of  additional  features  to  be 
tracked  increases  the  number  of  measurement  equations,  which  helps  wash  out  the  bearing 
angles  measurement  error. 


3.3.2  Modeling/Calibrating  the  Free  INS.  With  the  dynamics  from 
Subsection  3.3.1,  the  values  for  the  standard  deviation  cra  and  crg,  the  uncertainty  in  the 
bias  of  the  accelerometers  and  gyroscopes,  respectively,  are  set  such  that  the  free  INS  is  a 
navigation  system.  Since  the  dynamics  are  not  forced,  that  is,  there  is  no  controlled 
input,  the  calibration  is  performed  by  using  the  solution  to  the  Lyapunov  difference 
equation 

P(7  +  1)  =  AadP(7)Ajd,  0</<ZJV-l  (3.19) 
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with  the  initial  covariance  matrix 


09x9  0  0 

P(0)=  0  diag(cr2a,  cr2a,  cr2)  0  (3.20) 

0  0  diagicr2,  cr2,  cr2) 

Note:  During  1  hr  the  number  of  measurement  epochs  is  N=  360. 

Since  the  Lyapunov  difference  equation  is  linear,  there  is  a  linear  relationship 
between  the  uncertainty  in  the  sensors’  biases  variances  and  the  ensuing  uncertainty  in  the 
aircraft’s  x  position: 

Pi,i(LN)  =  acr2  +  ficr2  (3.21) 

where  the  coefficients  a  and  / 3  are  constants.  Therefore,  Eq.  (3.19)  is  solved  for  one 
non-dimensional  hour  twice  to  calculate  the  values  of  the  constants  a  and  (3.  The  first 
time,  cra  is  set  to  1  and  crg  is  set  to  0.  The  second  time,  cra  is  set  to  0  and  crg  is  set  to  1. 
Then  assigning  the  errors  in  the  accelerometers  and  gyroscopes  an  equal  role  in  the 
uncertainty  of  the  aircraft’s  position  at  the  nondimensional  time  360,  the  values  for  the 
nondimentionalized  variances  of  the  sensors’  biases  are  calculated  as 

=  — L  =  1.0912  x  10~5  (3.22) 

V2ff 

crg  =  =  9.0935  x  10~8  (3.23) 

Using  the  calculated  cra  and  <x„,  the  errors  of  the  free  INS,  <5x.,  are  generated  through  the 
solution  of  the  linear  difference  equation  (3.16)  with 

'  <5P(0)  ' 

<SV(0) 

dxa(0)  =  SV(0)  (3-24) 

df(0) 

v  Soj(0) 

v  '  15x1 
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where 


(dP^b)(0),  <5Pyb,(0),  dP|,b,(0)  ~  Af(03xi,  1  x  10-6I3) 
(5V^(0),hV;b)(0),hV'b,(0)  ~  iV(03xi,  1  x  10-16I3) 
(h^(0),^(0),^(0)  ~  A(03xl,  1  x  10_8I3) 

(5fib)(0), <>(0), df<b)(0))  ~  Af(03xl,  cr~I3) 

(&4b)(0),  dmJ^O),  ^ib)«»)  ~  iV(0M^I3) 

Thus,  it  is  assumed  that  the  initial  uncertainty  in  the  aircraft  position  is  ca.  1  [m],  the 
uncertainty  in  its  velocity  is  ca.  10-3  [mm/ sec],  and  the  uncertainty  in  pitch,  roll,  and  yaw 
is  a  ca.  20  arc  seconds.  The  ensuing  navigation  state  error  of  the  free  INS  is  given  by  the 
solution  of  Eq.  (3.16)  solved  over  the  planning  horizon  360L  -  1.  These  are  the  errors  of 
an  unaided/free  INS  and  will  serve  as  a  benchmark  to  be  compared  to  the  errors  when, 
using  SLAM  the  INS  is  aided  by  the  measurement  over  time  of  the  bearings  of  terrain 
features. 

3.3.3  Measurement  Equation.  The  relationship  of  the  inertial  position  and  attitude 
of  the  aircraft  to  that  of  the  ground  object/feature  P  is 

y 

z 

where  Xf  and  y  /  are  the  projections  of  the  ground  feature’s  respective  x  and  y  coordinates 
onto  the  focal  plane  of  the  camera  and  /  is  the  camera’s  focal  length  -  see  Figure  3.2.  For 

the  case  when  the  aircraft  flies  wings  level  at  a  constant  altitude  in  the  direction  of  the  x„ 
axis  and  the  Euler  angles  are  small,  the  DCM  for  relating  the  body  frame  to  the  navigation 
frame  is  given  in  Eq.  (3.5).  The  relationship  (3.25)  has  the  appearance  of  three  equations 
but  in  fact  has  the  strength  of  two  independent  equations.  The  first  two  equations  in  the 


Xp 

Xf 

y r 

\Rlos 

y/ 

^x2f+y2f+f2 

-f 

Zp 

(3.25) 
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Figure  3.2:  Measurement  Geometry  in  General  Position. 


relationship  given  by  Eq.  (3.25)  are  non-linearly  dependent  on  the  third.  Now,  the  third 
equation  yields 

xf 


Zn~Z  = 


\Rlos  I 


V* 2f+y2f+f2L 


0  0  1 


yf 

-f 


and  thus 


I  Rlos  I 


zp-z 


^xj+yj  +  f2 


0  0  1 


xf 

y/ 

-f 


(3.26) 
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Substituting  Eq.  (3.26)  into  Eq.  (3.25)  yields  the  two  measurement  equations  for  the  three 
dimensional  case: 


X 

Xp 

Zp-Z 

_  y  _ 

_  yP . 

xf 

0  0  1 


yf 


1  0  0 
0  1  0 


xf 

y/ 

-f 


Multiplying  out  the  matrices  yields 


X 

Xp 

1 

(  ^  rr\ 

* 

i 

'-s 

1 

_  y  _ 

_  yP . 

- f-exf  +  tof 

yf  +  xf\p  +  f(p 

and  nondimensionalizing  such  that 


xf 


x_l_ 

/’ 


yf 


yf 

f 


yields 


X 

Xp 

1 

xf  -  \Jryf  -  6 

_  y  _ 

_  yP . 

p  -1  -  dxf  +  (pyf 

yf  +  Xfip  +  (p 

Thus,  two  separate  nonlinear  measurement  equations  are  obtained 


xp  -  x  - 


~{zp  -  z) 


xf  -  *h>f  -  ° 
1  +  Oxf  -  4>yf 


}’p  -y  =  -( Zp  -  z) 


yf  +  if/Xf  +  (p 
1  +  Bxf  -  (pyf 


(3.27) 

(3.28) 


Due  to  the  small  angles  assumption,  the  denominator  in  Eqs.  (3.27)  and  (3.28)  can  be 
moved  up  such  that 


xp  -  X  ~  ~(zp  -  z)(xf  -  ifjyf  -  9)(  1  -  0xf  +  (pyf)  (3.29) 

yp-y~  -( Zp  -  z)(yf  +  Xfip  +  <p)(  1  -  Oxf  +  (pyf)  (3.30) 

Since  the  aircraft  is  using  ground  objects  to  aid  its  INS,  it  can  be  assumed,  without  loss  of 
generality,  that  the  terrain  elevation  is  known  and  zp=  0.  Due  to  the  small  values  of  the 
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angles,  when  the  former  fraction  is  distributed  out,  the  products  of  the  angles  are 
negligible,  yielding 

Xp  -  x  =  z[xf  -  0(1  +  x2)  +  pxfyf  -  ipyf ]  (3.31) 

yP  ~  y  =  z[yf  -  6xfyf  +  <p(  1  +  y2f)  +  ip xf ]  (3.32) 


Next,  perturb  the  states  and  the  measurements 


x  =  xc  -  5x 

e  =  ec- 86 


y  =  yc~  8y 

(f)  =  (pc  -  8<p 

}’p  =  yPc  -  8y 


z  =  zc~  8z 

ip  =  ifjc  -  Sip 


Xf  =  xfm-8xf  yf  =  yfm-8yf 

where  the  subscript  c  indicates  the  navigation  states  components  provided  by  the  free  INS 
and  the  subscript  m  indicates  measured  quantities.  The  calculation  of  the  “nominal” 
position  ( xpc,ypc )  of  a  geolocated  ground  feature  will  be  discussed  in  the  sequel.  Inserting 
the  perturbation  equations  into  the  measurement  Eqs.  (3.31)  and  (3.32)  yields 

xpc-xc  +  8x  -  Sxp  =(zc  -  Sz)(xfm-8xf  -  (6C  -  86)(  1  +  x2fm  -  2 xfmSxf  +  8x2) 


+  ( cpc  -  6<p)(xfm-Sxf)(yfm-Sy  f)  -  ( ipc  -  Sip)(yfn 


Due  to  the  small  error  in  the  measurements  and  the  small  angles,  the  products  of  these 
terms  can  be  neglected. 

+ &  _  =fe  -  szixf,-sx,  -  <a  -  m  i + 

(0c  8(l))Xjmyjm  ( Pc  8ip)yjm | 

Similarly,  in  the  second  measurement  equation 


yPc-yc  +  Sy  -  Syp  ={zc  -  Sz)\yfm-Syf  -  (9C  -  S6)(xfm-Sxf)(yfm-8yf)  +  (0C  -  8(p) 
( Tfm  -  2 y/mfyf  +  Syj)  +  ( ipc  -  8tp)(xfm-8xf)j 
=(Zc  ~  5z)(yfm-8yf  -  (6C  -  86)xfmyfm  +  (0C  -  8(p)(  1  +  y2fm) 

(<Ac  8ip)xfm\ 
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Moving  all  the  error  terms  to  the  Right  Hand  Side  (RHS)  of  the  equation  and  all  the 
non-error  terms  to  the  Left  Hand  Side  (LHS)  yields 


XpC  Xc  ZcYxfm  0C(1  “I"  ^fm)  (pcXfm.y fin  (pcy fm ]  — 

8x  -  ()z.  |  Xftn  9 c  ( 1  +  xj-in )  +  <bcXfmyfm  J  (3.33) 

+  89(  1  +  xfm)zc  S(pXfmyfmzc  "i"  Sij/yfmZc  +  8xp  —  z.<Sxf 

and 

y pc  Ji:  Zc\yfm  9cXfmy fm  +  (pci.  1  y fm)  ^Pcxfm\ 

-  dy  -  <fe[y/m  -  9cXfmyfm  +  0C(1  +  y^J  +  <Acx/m]  (3.34) 

+  66xfmyfmzc  40(  1  +  yfm)zc  8if/Xf,nZc  +  dy  ^  Zc8y f 

Finally,  nondimensionalizing  such  that 

xp  yp  Zp 

Xp  ^  h  yp~*h  Zp  ^  h’ 

we  also  note  that  the  nominal  nondimensional  altitude  is  z  =  1.  In  addition,  for  the  purpose 
of  covariance  and  Kalman  Filtering  analysis,  set  all  of  the  calculated  and  measured  values 
on  the  RHS  equal  to  the  nominal/true  values.  This  causes  all  of  the  angles  to  go  to  zero, 
simplifying  the  measurement  Eqs.  (3.33)  and  (3.34).  Also,  on  the  RHS  set  Xfm  :=  x/  and 
y/,„  :=  yf,  where,  in  view  of  the  nondime  ns  ionalization,  and  since  in  the  KF 
mechanization  in  each  measurement  epoch  t  is  the  time  elapsed  from  the  beginning  of  the 
epoch,  Xf  =  xp-  t,  Vf  =  yp  -  see  Figure  3.1.  Hence,  for  the  first  ground  object, 

Xf\{t)  =  1  -  t,  y/i(/)  =  yp i  and  for  the  second  ground  object  Xf2(t)  =  2  -  t,  y/2(0  =  yPi- 
Thus,  Eqs.  (3.33)  and  (3.34)  yield  the  linearized  measurement  equations 

Xpc  Xc  Zc\_Xfm  9 c ( 1  +  X~rjn)  +  (pcXfmy fm  (A cy fnA  ~ 

(3.35) 

-  8x  -  Xfdz  +  89(  1  +  x2f )  -  Xfyf8(p  +  VfSip  +  8xp  -  8xf 

and 


y pc  yc  Zc\y/m  9cXfmy fm  +  (pci  1  X-  V  f)n )  +  lpcXfm] 

(3.36) 

-  8y  -  8zy/  +  Xfyf89  -  8(p{  1  +  y2f)  -  Xf8ip  +  8yp  -  8y / 
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Hence,  the  time  dependent  observation  matrix  H(7)  during  a  measurement  epoch  with 
one  unknown  ground  feature  is 

r  -.t 

-1  0 

0  -1 

-xf  -yf 

0  0 

0  0 

0  0 

-xfyf  -(1  +y2f) 

1  +  x )  xfyf 

H  ud)  =  yf  -xf  (3.37) 

0  0 

0  0 

0  0 

0  0 

0  0 

0  0 

1  0 

0  1 

where  the  subscript  u  indicates  that  the  position  of  the  ground  object  being  tracked  is 
unknown.  The  nondimensional  measurement  error  is  [ 6xf ,  8yf\ 7  . 

Since,  for  the  sake  of  observability  [9],  in  each  measurement  epoch  two  ground 
objects  will  be  tracked,  there  will  be  two  subscripts  1  and  2.  The  first  corresponds  to  the 
ground  object  that  is  closer  to  the  aircraft  and  the  second  to  the  ground  object  that  is 
further  away.  In  the  first  measurement  epoch  it  is  assumed  that  both  ground  objects  are 
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known.  If  both  ground  objects  are  known,  the  observation  matrix  is 


H kkil)  - 


-1 

0 

-l 

0 

0 

-1 

0 

-1 

-Xfl 

-yf  i 

~Xf2 

-yn 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

-Xfiy/ 1 

-(1  +>/,) 

-Xfiyfi 

-('  +>/ 

1  +  Xj| 

xnyn 

l+xj2 

Xfiyn 

>’/l 

~Xfl 

yn 

-Xfl 
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0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

(3.38) 


where  the  subscript  k  indicates  that  the  position  of  the  ground  object  is  known.  In  the 
second  measurement  epoch  a  new  ground  feature  is  acquired  so  that  in  the  field  of  view  of 
the  camera  there  is  one  known  and  one  unknown  ground  object.  When  there  is  one  known 
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and  one  unknown  ground  object,  the  observation  matrix  is 


H  ku(l)  = 


-1 

0 

-l 

0 

0 

-1 

0 

-1 

-Xfl 

-yn 

-Xf2 

-yn 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

-Xfiyn 

-(i  +y2fl) 

-Xfiyfi 

-(i  +y)2) 

1  +  x1^ 

xf\y.f\ 

1  +  X2p 

xnyn 

>’/l 

~Xfl 

yn 

-Xfl 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

(3.39) 
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Finally,  starting  at  measurement  epoch  three  when  neither  ground  object’s  position  is 
known,  the  observation  matrix 


-1 

0 

-l 

0 

0 

-l 

0 

-1 

~Xf\ 

-y/ 1 

~Xf2 

-y/  2 

0 

0 

0 

0 
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0 

0 

0 

0 

0 

0 

0 

-W/1 

-(i  +y2fl) 

-Xfiyfi 
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(3.40) 


Two  ground  objects,  P\  and  P2  are  tracked.  The  measurements  z  given  to  the  Kalman 


Filter  during  the  nth  measurement  epoch  at  each  time  step,  /=1,2=L,  are  generated  from 
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the  LHS  of  Eqs.  (3.35)  and  (3.36)  as 

Xplc  Xc  Zc(.Xfm  1  0C(1  +  Xfm i)  “I"  (pcXfmiy fml 

A7>lc  AA  Zc(y fm\  dcXfmiy fml  +  0c(l  “1“  A  //// 1  )  ftcXfml) 

Xp2c  *C  Zc(Xfm 2  0<?(1  Xfm?)  ficXfmiy fin?  dy fml) 

y p2c  AA  Zciy fm2  QcXfm?y fm?  “A  (pci  1  “1“  y fm?)  ^ cXfrril) 

where 

x/i(f)  =  1  —  r,  x/2(0  =  2-t,  t  -  IAT,  1  <  l  <  L 

and 


Xfm  1  -  */l(0  +  £l>  Xfm?  ~  Xflit)  +  £2, 

ft  ~N(0,cr2),  ft  ~  N(0,  a1), 

y /ml  =  y/1  +  ?7l,  y f m2  -  y/2  +  ?72> 

rn-NiO'O*),  tj?  ~  7V(0,  cr2) 

where  <r=  I  x  10-3  (for  a  9  mega  pixels  camera  with  an  aspect  ratio  of  1).  The  calculated 
navigation  state  is  output  by  the  free  INS  and  given  by 

Xc  =  XF  =  X  +  C>XF 

where  x  is  the  true  (nondimensional)  navigation  state  given  by 

x  =  [  f  0  1  1  00000  f 

and  dxF  is  the  navigation  state  error  of  the  free  INS.  Thus,  the  sequence  dxF  is  obtained  as 
follows: 

'  <5P(0) 

<5V(0) 

dxF(/  +  1)  =  Aad<5xF(ft,  £xa(0)  =  dT(0) 

<5f(0) 
v  M0)  , 

v  '  15x1 
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1  =  0,...,  LN  -  1 


where 


(dP^b)(0),  <5Pyb,(0),  dP|,b,(0)  ~  A(03xl,  1  x  10-6I3) 
(5V^(0),5V;b)(0),hV'b,(0)  ~  jV(03xl,  1  x  io-16i3) 
(^b)(0),^b)(0),^b)(0)  ~  iV(03xl,  1  x  10_8I3) 
(^b)(0),^b>(0),^b,(0))  ~  N(03x1,^I3) 

(^b,(0),^b)(0),^ib)(0))  ~  iV(03xi,^I3) 

During  a  measurement  epoch  xpic,  xp2c,  yp2c ,  and  yp2c  which  feature  in  measurements 
z  given  to  the  KF  are  held  constant.  At  the  end  of  the  measurement  epoch  they  are  updated 
for  the  next  measurement  epoch.  They  are  calculated  according  to  Tables  B.l  -  B.3  in 
Appendix  B . 


3.3.4  Synthesis  of  the  Measurement  Sent  to  the  KF  in  Epoch  n.  In  epoch  1, 
xPic  =  1,  >Vic  =  -yp,  and  xp2c  =  2,  yp2c  =  yp.  In  epoch  2,  xpU.  =  2,  yplc  =  yp. 

Consider  measurement  epoch  n,  3  <  n  <  N,N= 360.  In  total,  n  +  1  ground  features 
are  used,  n  -  1  new  ground  features  are  geolocated. 

The  meaning  of  xpl  and  ypc  used  during  epoch  n  >  2  to  calculate  the  measurement 
given  to  the  KF  -  we  refer  to  Eq.  (3.41):  they  are  kept  constant  during  the  measurement 
epoch  in)  and  were  calculated  at  the  conclusion  of  epoch  (n  -  1 )  using 
Eqs.  (3.27)  and  (3.28)  and  setting  therein  zp  =  0: 


X  +  2 


Xf  -  fyf  -  6 
1  +  Oxf  -  (pyf 


(n) 

yPic 


y  +  z 


yf  +  if/Xf  +  (p 
1  +  Qxf  -  (pyf 


(3.42) 

(3.43) 
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Since  the  scenario  considered  is  wings  level  flight  at  constant  altitude,  in 
Eqs.  (3.42)  and  (3.43)  set 

x  =  n  —  1  +  8xF\n-[  -  6x |„_i, 
y  =  SyF\n-\  -  8y\n- 1, 

Z=  1  +  dZF\n-l  ~  8z\n-l, 

ln-1  -  <fyU, 

0  =  <50fL-1  - 
<P  =  <50f|„_i  -  d0|„_i 

and  since  at  the  end  of  an  epoch  the  newly  acquired  ground  feature’s  Xf  =  2 ,  yy  =  ±yp,  in 
Eqs.  (3.42)  and  (3.43)  set 

xf  =  2  +  t, 

}\f  =  —}’p  +  0,  0  ~  N(0,  cr 2) 

where 

cr  =  -  x  10  3, 

8xF\n-i  =  SxF((n  -  1  )L),  Sypln-i  =  SyF((n  -  1  )L) 

whereas 

&U  =  dVn'r)(L),  hy|„_,  =  ^"“V) 

and  the  same  applies  to  the  navigation  states  z,  <A,  9,  (p.  In  addition,  at  the  end  of  the 
measurement  epoch  n—  1,  set 

(»)  .  (»-d  _  Sx(n~1}(D 

Xplc  -_-lp2c  °->2 

(«)  (n- 1) 

>plc  =y'p2c  ~  ftp 2  (L)’ 

77  =3,  •  •  •  ,  360 
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t  =  n-\+AT*l,  l  -  1,  •  •  •  ,L,  AT  =  l/L 

and 

Xflm  =  Xfi  +tju  gi~  N( 0,  CT2), 

Xf2m  =  Xf2  +  £2  ~  N(0,  cr2), 

>’/!m  =  ±yp  +  r/\,  771  ~  N(0,cr2), 
y f2m  =  +>’p  +  772,  772  ~  A7(0,  cr2) 

where,  during  the  measurement  epoch  (n) 

Xf\  —  1  —  l AT,  Xf2  —  2  —  l AT,  l  —  1,  •  •  •  ,  L 

3.4  Performance  of  Aided  INS 

The  performance  of  the  aided  INS  is  evaluated  for  the  nominal  scenario  described  in 
Section  3.2  and  illustrated  in  Figure  3.3.  The  aircraft  is  flying  wings-level,  at  a  constant 
speed  with  ground  features  spaced  at  one  kilometer  apart.  The  first  two  ground  features 
are  known  and  the  aircraft  starts  one  kilometer  behind  the  first  ground  feature.  In  the 
observation  matrices  H, 


Xf\(t)  =1—7,  Xf2(t)  =  2-7,  0  <  7  <  1 
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If  ground  features  are  arranged  in  a  straight  line  on  the  positive  x„  axis,  then  in  the  H 
matrix 


y/i(0  =  >’/2(0  =  0 

However,  if  the  ground  features  are  laterally  staggered  at  equal  distance  yp  about  the 
positive  xn  axis,  then  in  the  H  matrix 


y/ 1(0  =  yP  yfi(t )  =  -yp 


Xn,  Epoch  2  Xn? 

Known  Ground  Points  IUnknown  Ground  Points 

>  Aircraft  using  measurement 

■ - —  —  — >  Aircraft  geo-locates  unknown  ground  object 

Aircraft  stops  using  ground  object 


Figure  3.3:  In  the  first  measurement  epoch  the  two  ground  objects’  position  are  known, 
but  in  the  second  measurement  epoch  there  is  one  known  and  one  unknown  ground  object, 
where  the  unknown  object’s  position  was  estimated  by  the  aircraft  at  the  end  of  the  first 
epoch.  From  epoch  3  onward  both  ground  objects’  locations  are  estimated  using  the 
aircraft’s  navigation  state  at  the  end  of  the  preceding  epoch  and  therefore  are  not  exactly 
known. 


41 


3.4.1  Initialization  of  the  KF.  In  the  first  measurement  epoch  the  Kalman  Filter  is 
initialized  according  to  Eq.  (3.44) 

(<Sx>))(1)  =  015xl  (3.44) 

while  the  covariance  matrix  is  initialized  as  (P(0))n)  by  using  P  as  in  the  equation 
following  Eq.  (3.24)  where  it  is  assumed  that  the  position,  velocity,  and  orientation  are 
known  with  an  uncertainty  of  a  =  1  x  10-6,  b  =  lx  10“16,  and  c  =  1  x  10-8,  respectively 
as  in,  Eq.  (3.45). 

A  0  0  0  0 

0  5  0  0  0 

(P(0))(1)  =  o  0  C  0  0  (3.45) 

0  0  0  D  0 

0  0  0  0  5 

L  J  15x15 

where 

A  =  diag(a,a,a ) 

B  =  diag(b,b,b ) 

C  =  diagic,  c,  c ) 

D  =  diag{cr2a,  cr2a,  erf) 

E  =  diagicr2,  cr2,  cr2) 

The  superscripts  in  (dXg(0))(1)  and  (P(0))(1)  represents  the  epoch  number  (epoch  1  in  this 
case).  The  three  accelerometer  and  the  three  gyroscope  set  are  of  the  same  quality:  the 
random  biases  in  the  sensors  are 


dfx  ~  N(0,  cr2a) 

fify  ~  N(0,  cr2a) 

6fb~N(0,cr2a ) 

6(obx  ~  N(0,  cr2) 

6cob  ~  N(0,  cr2) 

6ojhz  ~  N (0,  <t2) 

In  each  measurement  epoch  the  KF  is  re-initialized.  Since  in  measurement  epoch  n,n>  2, 
one  navigates  off  a  newly  acquired  ground  feature,  the  KF  is  being  re-initialized  as 
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follows:  In  epoch  n  =  2  the  KF  state  is  a  17  x  1  vector  initialized  as 


(d.r(0)15xl)(2)  =  ( 0x(L)15xl)u\  6Xla(P)  =  0,  Syp2(0)  =  0, 


\(b 


and  (P(0)) 


(2) 


(P(L)15xl5)(1)  Pl5x2 

(pS.5)7  11 


2x2 


J  17x17 


where  the  covariance  of  the  position  error  of  the  newly  acquired  ground  feature  and  their 
cross  correlation  terms  are  calculated  using  Eqs.  (3.31)  and  (3.32).  The  ground  position  xp 
and  yp  are  isolated  from  Eqs.  (3.31)  and  (3.32)  as  follows: 


xp  =  x  +  z[Xf  -  0(1  +  x2f)  +  (pXf-yf  -  t/ryf]  (3.46) 

yP  =  y  +  z[yf  -  9xfyf  +  0(1  +  y2f)  +  i//xf]  (3.47) 


Perturbing  Eq  (3.46)  yields 

6xP2  =Sx  +  8z[Xf  -  0(1  +  x2)  +  (pxyyy  -  0y/]  +  z[8xy  -  <50(1  +  x2)  -  2 Xf08xf  +  Xf-yf8(p 
+  ( fixfdyy  +  (pSxyyy  -  yySifr  -  if/Syy] 

Setting  z  =  1,  0  =  0  =  0  =  0,  x/  =  2,  y/  =  ±yp,  Sxy  =  0,  and  Sy/  -  rj ,  where 

t~N(0,(r2),  tj-N^ct2) 


we  obtain:  at  the  beginning  of  epoch  2  the  error  in  the  newly  acquired  feature’s  position 

Sxp2(0)  =  Sx  +  2Sz  -  5 86  ±  2 yp8<p  +  yp8if/  +  ^  (3.48) 

Similarly,  perturbing  Eq.  (3.47)  yields 

8yp 2  =Sy  +  8z[}'f  -  6xfyf  +  0(1  +  y2)  +  i /ay]  +  z{8y  f  -  xfyfS6  -  9xf8yf  -  9yf8xf 
+  2  yf(p8yf  +  00(1  +  y2)  +  i/zSxf  +  X/SiJ/  ] 

That  is,  at  the  beginning  of  epoch  2  the  error  in  the  newly  acquired  feature’s  position 

dyP2(0)  =  Sy  ±  yp8z  +  2 yp89  +  (1  +  yp2)8(p  +  2 8iJj  +  rj  (3.49) 
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Eqs.  (3.48)  and  (3.49),  can  be  re-written  to  give  the  x  and  y  position  of  the  second  ground 
feature  at  the  beginning  of  an  epoch  as 


6x(p 5(0)  =6x(L)(n~l)  +  2 Sz(L)(n~l)  -  56e(Lf^  ±  2 yp6cf>(L)(n-l)  +  ypdiJ/(L)(n~l)  +  f  (3.50) 
dy("}( 0)  =dy(L)("-1)  +  5(p(L)(n~V)  +  2d>(L)('i-,)  ±  ypSz(L)ln~^  +  2y/)d0(L)(,!-|) 

(3.51) 

+  vJd0(L)("-1)  +  7/ 

3.4.2  Kalman  Filter.  The  INS  aiding  scheme  is  as  illustrated  in  Figure  3.4.  The 
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Figure  3.4:  INS  Aiding  Using  a  Kalman  Filter 


free  INS  outputs  the  calculated  navigation  state,  xc,  which  is  the  sum  of  the  true 
navigation  state,  x  and  the  errors  in  the  navigation  states  of  the  free  INS,  dx.  The  aircraft’s 
camera  generates  a  noise  corrupted  measurement  of  the  free  INS  navigation  state  and 
upon  linearizing  the  measurement  equation  a  measurement  of  the  navigation  state’s  error 
is  obtained,  as  shown  in  Figure  3.4.  The  results  of  this  operation  gives  the  measurement  z, 
which  is  provided  to  the  KF.  Using  this  information,  the  KF  calculates  an  estimate  dx  of 
the  free  INS  navigation  state  error  dx,  which  is  subtracted  from  the  free  INS  navigation 
state  xf  =  x  +  dx  to  yield  the  calculated  “clean”  navigation  state  xc  =  x  +  dx  -  dx.  Thus  in 
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effect,  the  KF  removes  the  error  from  the  calculated  navigation  state,  as  shown  in 
Figure  3.4. 

In  the  measurement  epoch  1,  the  positions  of  both  tracked  ground  features  are 
assumed  known.  Therefore,  in  epoch  1  the  observation  matrix  Hkk(/)4xl5,  and  the 
dynamics  matrix  Aadl5xl5  are  used.  For  both  scenarios  (ground  features  in  a  straight  line 
and  staggered),  the  one  hour  flight  duration  results  in  360  nondimensionalized  seconds 
resulting  in  360  epochs.  Each  non-dimentionalized  second  is  10  [5].  Sampling  at 
0.2  [//z]  results  in  L  =  2  bearing  measurements  in  each  measurement  epoch.  In  the  first 
epoch,  the  error  state  estimate  is  propagated  for  two  steps.  The  propagate  equations  of  the 
Kalman  filter  [8]  are  as  follows: 

dx/+1  =  A  aA5\]  (3.52) 

p  r+1  =  AadP/+Ajd  (3.53) 

and  the  error  state  estimate  is  updated  using  the  update  equations  of  the  Kalman  filter  [8] 

*0+ 1  =  6xl+l  +  K;+i[Z;+i  -  H/+nfx/+1]  (3.54) 

where  the  Kalman  gain  K  is  given  by 

K/+1  =  +  RF1  (3.55) 

and 

P;+1  =(I-K/+1H/+1)P/"1  (3.56) 

In  Eq.  (3.55)  R  is  the  covariance  matrix  of  the  bearings  angles’  measurement  error.  It 
corresponds  to  one  pixel  measurement  error  in  the  camera’s  focal  plane: 

Xfm  =Xf  +  Sxf,  yfm  =  y f  +  Syf 

~N(  0,R) 


8xn 
6yfl 
6xf2 
,  fyf  2 
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For  a  small  UAV,  a  9  Mega  pixel  camera  with  an  aspect  ratio  of  1  is  assumed  and  therefore 
cr  =  |  x  10-3  (error  in  1  pixel)  and  the  nondimens ional  measurement  error  covariance 
matrix 

I  0  0  0 

o  ;  o  o 

R  =  9  x  1(T6  (3.57) 

0  0  ±  0 

0  0  0  | 

At  the  conclusion  of  the  first  two  steps/the  first  measurement  epoch,  the  first  ground  object 
is  dropped  from  consideration,  and  a  new,  unknown  ground  object  is  brought  in  as  shown 
in  Figure  3.3.  In  the  second  measurement  epoch,  the  second  ground  object  from  epoch  1 
becomes  the  ground  object  1  in  epoch  2,  whose  position  is  perfectly  known  so  that  at  the 
beginning  of  epoch  2,  (Sxp2( 0))(2)  =  0,  (d'y/)2(0))<2>  =  0,  and  the  newly  acquired  ground 
object  becomes  ground  object  2,  whose  position  during  the  measurement  epoch  2  is 
2  +  8x(L) i,  where  8x(L) i  is  the  x-component  of  the  free  INS’s  position’s  error  at  the  end  of 
the  first  measurement  epoch.  Thus  the  next  time  block  requires  the  use  of  the  augmented 
dynamics  matrix  Aadl7xl7  from  Eq.  (3.18)  and  the  observation  matrix  Hku(/)4xi7. 

At  the  start  of  the  second  measurement  epoch,  the  error  state’s  covariance  matrix  at 
the  end  of  epoch  1,  (P(L))(1),  is  transitioned  from  a  15  x  15  to  a  17  x  17  matrix,  while 
including  the  correct  cross-covariance  terms.  This  is  done  as  follows:  when  transitioning 
from  two  known  ground  features  to  one  known  and  one  unknown  ground  feature,  the  new 
covariance  matrix  at  the  beginning  of  measurement  epoch  2  is 
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where  the  elements  of  the  matrix  block  pgx2  are  obtained  using  Eqs.  (3.50)  and  (3.51)  as 
follows: 

P?l i  =E (sx(L)%  .  6* 0)®) 

=E (sx(L)^  .  ( 6x(L){l)  +  2 &(L)(1>  -  5d<9(L)(I)  ±  2 y/0(L)(1)  +  yp5ip(L)<]>  +  £)) 
=P(£)g  +  2P(L)g  -  5P(L)g  ±  2>'pP(L)^  +  ypP(L)g 
p%_  =E (dv(L)2  .  <fc(0)®) 

=E(d><L)^  .  (d.r(L)a)  +  2&(L)(1)  -  5 86{L)a)  +  2ypS<p(L)<v>  +  ypd<A(L)(1)  +  £)) 

=P(£)g  +  2P(L)g  -  5P(L)g  ±  2ypP(L)g  +  ypP(L)g 


(2) 
P 16,/ 


=P(^)i 


(1) 


+  2P(L)g  -  5P(L) 


(i) 


2ypP(L)£+ypP(L)9, 


(i) 


for  i=l,. . .  ,15 
Similarly, 


(2) 
P 17,/ 


=p(o2 + p<i), , 


(i) 


+  2P(l)£  ±  yPP(L)' ;;  +  2ypP(D^;  +  ^p(l) 


un  T 


7,( 


for  i=l,. . .  ,15 

The  first  and  second  diagonal  terms  of  the  matrix  11(0)^  are  the  respective 
uncertainty  in  the  x  and  y  position  of  the  new  ground  object,  and  it  is  obtained  using  Eqs. 
(3.50)  and  (3.51)  as  follows: 


n(0)g  =  P(O) 


(2) 

16,16 


where 

P(0)S,6  =e(&(0)2  .  <5*(0)“>) 

=e((&c(L)(1)  +  2 6z(Lf]  -  5 66(L)W  ±  2 yp6(p{L)m  +  yp6i^(L)(l)  +  g)  .  (6x(L)a) 
+  2&(L)(1)  -  566(L){1)  ±  2 yp8<f>(L)a)  +  y,M(L)il)  +  £)\ 


47 


=P(£)S  +  4P(L)g  +  25P(L)g  +  4P(L)g  -  10P (L)g  -  20P(L)g  +  4yJP(L)g 
+  yJP(L)g  ±  4y/;P(L)g  +  2ypP(L)g  ±  8ypP(L)g  +  4ypP(L)g  +  20ypP(L)g 
±10^P(L)g-4^P(L)«+(r| 
n(0)g  =  P(0)g]7 
where 

P(0)g17  =E(tfy(0)®  •  5y(0)g) 

=e((^(L)(1)  +  b>(L)(l)  +  25tA(L)(1)  +  >v&(L)(1)  +  2y/<9(L)(1)  +  y2p8(/>(L)w  +  77) 

.  (dy(L)(l)  +  &KL)(1)  +  2<fyr(L)(1)  ±  yp6z(LfX)  +  2 yp88{L)m  +  y2p8(f>(L)(1)  +  77)) 

=P(L)g  +  P(L)g  +  4P(L)g  +  2P(L)g  +  4P(L)g  +  4P(L)g  ±  2>VP(L)g 

+  4vpP(L)g  +  2y2pP (L)g  ±  2gP(L)g  +  4yJP(L)g  +  2yJP(L)g  +  /pP(L)g 

±  4vpP(L)g  +  8ypP(L)g  +  4^P(L)g  ±  ypP(L)g  +  2ypP(L)g  ±  ypP(L)g 

+  vJP(L)g  -  2yJP(L)g  +  2vpP(L)g  -  2y2pV(L)%  +  4yJP(L)g  +  oj 
Finally,  the  off-diagonal  terms  of  the  matrix  block  II2x2  are  obtained  as  follows: 

ni, 2  =  n21  =  P(0)g17 

where 

P(0)g,7  =E^(0)®  •  tfy(0)®) 

=E^(d.r(L)(1)  +  2Sz(L)(V)  -  5 86(L)a)  ±  2 '.yp8<p(L)m  +  yp8ip{L)(X]  +  £) .  (8y(L)(l} 

+  8(f)(L)m  +  2 8i//(L)(l)  ±  yp8z(L){l)  +  2yp86(LjV)  +  y2p8cp(L)a)  +  77)) 

=P(L)g  +  2P(L)g  -  5P(L)g  +  P(L)g  +  2P(L)g  -  5P(L)g  +  2P(L)g  +  4P(L)g 
-  10P(L)g  ±  ypP <L)g  +  2ypP(L)g  +  ypJ> '(L)g  ±  2ypP(L)g  +  4ypP(L)g 
+  2y2pP (L)g  +  5ypP(L)g  ±  10y/;P(L)g  -  5y;P(L)g  ±  2ypP(L)g  ±  2y/;P(L)g 
±  4y,P(L)g  +  2yJP(L)g  -  4y;P(L)g  ±  2gP(L)g  +  ypP(L)g  +  ypP(L)g 
+  2y„P(L)g  -  yJP(L)g  +  2y;P(L)g  +  gP(L)g 
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where,  recall,  <f  and  rj  are  the  uncertainties  caused  by  the  error  in  the  LOS  angle 
measurements.  It  is  because  of  the  correlation  of  the  errors  in  the  aircraft  navigation  state 
x  and  the  new  ground  object’s  position  state  xpi  that  the  first  row  and  column  from 
(P(L))|'5^|5  are  used  in  the  calculation  of  their  respective  positions  in  (P(0))(17xl7.  Since 
there  is  no  correlation  between  the  LOS  error  of  the  camera  and  any  of  the  INS  navigation 
or  bias  states,  cr2  is  not  added  in  the  calculations  of  any  of  the  entries  of  the  covariance 
matrix  except  P(0)  |2)  |6  and  P('0)l|7'  |7.  The  same  holds  true  for  the  navigation  state  y  and  the 
new  state  yp 2.  The  covariance  matrix  is  then  propagated  in  the  same  manner  as  in  the  first 
epoch,  following  Eqs.  (3.53)-(3 .56),  with  the  proper  substitution  of  the  initial  covariance, 
(P+(0))|2)x17  for  (P(2))(115x15,  dynamics,  Aadl7xl7  for  Aadl5xl5,  and  observation  matrices,  Hku 


for  Hkk. 

The  state  estimate  is  transitioned  by  augmenting  the  state  estimate  at  the  last 
measurement  step  L  with  zeros  as 


wtm™  = 


(3.59) 


17x1 


'  («:<«, 5*,)ai ' 
v  02xi 

The  transition  at  the  beginning  of  the  third  epoch  from  one  known/one  unknown  to 
two  unknown  ground  objects  follows  the  same  pattern  as  incorporating  the  first  unknown 
ground  object.  Now 


(P(0)) 


(3) 


(P  (L)  17x17 
(P2X17) 


y2)  p(3) 


17x2 

112x2 


(3.60) 


J19xl9 


where  the  elements  of  the  matrix  block  P(0)(,37  2  are  obtained  using  Eqs.  (3.50)  and  (3.51) 


as  follows: 


P(0)^,  =E (d.v(L)g  .  6x(0)%) 

=E (d.v(L)g  .  (6x{L)i2)  +  2 8z(L){2)  -  5 d<9(L)(2)  ±  2 yp6cf>(L)(2)  +  yfM(L)a>  +  £)\ 
=P(L)(12;  +  2P(L)g  -  5P(L)'2;  ±  2ypP(L)f  \  +  ypP(L)f\ 
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P(0)S2  =E (sy(L)%  .  ^(0)g) 

=E (dy(L)g  .  (d.*(L)(2)  +  2 8z(Lf  ]  -  5 66(L)™  ±  2 yp«50(L)<2>  +  »#(L)(2)  +  £)) 
=P(L)g  +  2P(L)g  -  5P(L)g  ±  2>'/;P(L)g  +  ypP(L)g 


P(0)g,  =P(L)g  +  2P(L)g  -  5P(L)g  ±  2ypP(L)%  +  »P(Lg 

for  i=l,. . .  ,17 
Similarly, 


\(2) 


\(2) 


42) 


\(2>  T- 


(2) 


P(0)g,  =P(L)g  +  P(L)g  +  2P(L)g  ±  »P(L)g  +  2y/,P(L)g  +  y;P(L) 


\(2) 


(2) 


42)  ^ 


44) 


44) 

7,( 


for  i=l,. . .  ,17 

The  first  and  second  diagonal  terms  of  the  matrix  block  n(0)g9  are  the  respective 
uncertainty  in  the  x  and  y  position  of  the  new  ground  object,  and  they  are  obtained  using 
Eqs.  (3.50)  and  (3.51)  as  follows: 

n(0)g  =  P(0)®4 


where 

P(0)S,4  =E(fa(0)®  •  tfVO)®) 

=E((d.v(L)(2)  +  2Sz(L)a>  -  5S6(L)(2)  ±  2 yp6(p(L)(2)  +  yp6t//(L){2)  +  f) .  ( 8x{L)(2) 

+  2 Sz(L)(2)  -  5 66(L){2]  ±  2yp8m{1)  +  y,M(L){2)  +  £)) 

=P(L)g  +  4P(L)g  +  25P(L)g  +  4P(L)g  -  10P(L)g  -  20P(L)g  +  4yJP(L)g 
+  >’2P(L)g  ±  4ypP(L)g  +  2ypP(L)g  ±  8ypP(L)g  +  4ypP(L)g  +  20ypP(L)g 
±  10y/;P(L)g-4gP(L)g  +  (g 
n(0)g  =  P(0)g]7 


where 

P(0)®,7  =e(«K0)®  .  <5y(0)g) 
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=e((^(L)(2)  +  8<P{L)(1)  +  2 cty(L/2)  ±  ypSz(L)(2>  +  2ypd6(L)(2)  +  y2b0(L)(2)  +  77) . 

(< 5y(L){2)  +  Scf>(L)i2)  +  25iA(L)(2)  ±  ypSz(L)(2)  +  2 yp86(L){2)  +  y2d0(X)(2)  +  77)) 
=P(L)g  +  P (L)g  +  4P(L)g  +  2P(L)g  +  4P(L)g  +  4P(L)g  ±  2ypP(L)g 
+  4yPP(L)g  +  2y2P(L)g  ±  2gP(L)g  +  4gP(L)g  +  2yJP(L)g  +  gP(L)g 
±  4>’pP(L)g  +  8>’pP(L)g  +  4y2P(L)g  ±  >y,P(L)g  +  2ypP(L)g  ±  ypP(L)g 

+  >JP(L)g  -  2y2P(L)g  +  2v,P(L)g  -  2yJP(L)g  +  4yJP(L)g  +  m2 
Similarly  the  off-diagonal  terms  of  the  matrix  II(0)g,  are  obtained  as  follows: 

n(0)g  =  n(0)g  =  P(0)gl7 

where 

P(0)Si7  =e(^0)®  •  «0)®’) 

=E((d.r(L)(2)  +  2 &(L)(2)  -  5 86(L){2)  ±  2 '.yp8<p(L)(2)  +  yp8<p(L)(2)  +  .  (dy(L)(2) 

+  8(P(L){2)  +  2Si//(L)a)  ±  yp8z{L){2)  +  2yp5G(Lf)  +  y;d0(L)(2)  +  77)) 

=P(L)g  +  2P(L)g  -  5P(L)g  +  P(L)g  +  2P(L)g  -  5P(L)g  +  2P(L)g  +  4P(L)g 

-  10P(L)g  ±  ypP(L)g  +  2ypP(L)g  +  y2P(L)g  ±  2ypP(L)g  +  4ypP(L)g 

+  2y2P(L)g  +  5y,P(L)g  ±  10ypP(L)g  -  5y2P(L)g  ±  2y,P(L)g  ±  2y/)P(L)g 

±  4ypP(L)g  +  2y2P(L)g  -  4y;P(L)g  ±  2yJP(L)g  +  ypP(L)g  +  ypP(L)g 

+  2y/,P(L)g  -  y2P(L)g  +  2y;P(L)g  +  gP(L)g 
In  the  previous  epoch,  the  unknown  object’s  position  was  (xp2,yP2),  but  when  it 

transitioned  to  being  the  closer  ground  object,  all  of  its  cross-covariances  goes  with  it. 
Because  (P(0))g  16)  and  (P(0))g17  show  the  uncertainty  of  the  closest  object  to  the 
aircraft,  the  entirety  of  (P(0))(135)xl5  is  directly  translated  to  the  upper-diagonal  section  of 
the  new  covariance  matrix.  Substituting  Huu  for  Hku,  and  using  the  Aadl9xl9  dynamics 
matrix,  the  covariance  was  propagated  according  to  Eqs.  (3.53)-(3.56).  These  matrices 
were  used  for  the  remainder  of  the  measurement  epochs. 
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=P(L)(/71)  +  2P(L)£"1)  -  5P(L)g"r1}  ±  2ypP(L)(;:l)  +  ^P(L)J:1) 

P(0)g.  =e(wO)$  .  ^(L)^-1}) 

=e|(^(L)("-1)  +  c 50(L)("-1}  +  2Si//(L)(n~l)  ±yIML)(,,-l)  +  2 yp<50(L)(B"1)  +^<50(L)O,"1) 

+  77)  .  <ty(X)J,2_1)) 

=P(L)J-1)  +  P(L)571}  +  2P(L) J"1}  ±  ^P(L)J-J)  +  2^P(L)J71)  +  vJP(L)7-1} 
for  i=l,. . .  ,15 
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The  first  and  second  diagonal  terms  of  the  matrix  n(0)^4  are  the  respective 
uncertainty  in  the  x  and  y  position  of  the  first  ground  object,  and  they  are  obtained  by 
transplanting  the  uncertainty  in  the  x  and  y  position  of  the  second  ground  object  from  the 
previous  epoch  as: 

P(0)«6  =11(0)“  =  pcdSK 

0(0)“  l7  =n(0)“  = 

The  third  and  fourth  diagonal  terms  of  the  matrix  11(0)^  are  the  respective 
uncertainty  in  the  x  and  y  position  of  the  second  ground  object,  and  they  are  obtained  by 
using  Eqs.  (3.50)  and  (3.51)  as  follows: 

n(0)“  =  P(0)“l8 

where 

P(0)S„  =E(fa(0)<"> .  <5*(0)“) 

=E^(<5x(L)("_l)  +  2 -  566(L)("-l}  +  2 yp6(p(L){n-l)  +  y  p8ij/{L)(n~l)  +  £) 

.  ( 6x{L)(n-l)  +  26z(L)(n~]>  -  56d(L)in-1)  ±  2 y/<KT)(,,“1)(d.v(L)°!~1)  +  26z(L)(n~l) 
-  5 8e{L)(n~l)  +  £)) 

=P(L)(”“1)  +  4P(L)J“ 1}  +  25P(L)J“ 1}  +  4P(L)(”“ 1}  -  10P(L)(”8 1}  -  20P(L)f~1) 

+  4yJP(L)^1)  +  yp-P(L)%-l)  ±  4y/,P(L)(;;7-1)  +  2ypP(L)(”-1)  ±  8ypP(L)^1) 

+  4ypP(L)J-1)  +  20ypP(L)%l)  ±  10ypP(L)(”-1}  -  4yJP(L)^1)  +  o* 
n(0)g  =  P(0)g19 
where 

P(0)S„  =e(^(0)<"»  •  <5X0)2) 

=e|(0v(Li",~|1  +  S<p(Lf-"  +  2<5<KL)<""1)  ±  yp6z(L)(’-,>  +  2y,<Se(L),"“1’  + 

+  77) .  (dy(L)("_1)  +  d0(L)(”_1)  +  2 d</<L)("-1,  ±  ypSz(L)(n~r>  +  2 yp86(L){n-l) 
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+  y2p6<p(L)(n-l)  +  Tl)) 

=P(L)J2_1)  +  P(L)^7_1)  +  4P(L)J9_1)  +  2P(L)J7_1)  +  4P(L)2'9 1}  +  4P(L)J“1) 

±  2ypV(L)(3n-V)  +  4vpP(L)7"”l)  +  2;yJP(L)£-1)  ±  2yJP(L)J7-1)  +  4yJP(L)^1) 
+  2vJP(L)^1}  +  yJP(L)£1)  ±  4ypP(L)J-1)  +  S^PIL)^  +  4^P(L)^1) 

±  ypP (L)J-1}  +  2JpP(L){f-1)  ±  ^P(L)J- 1}  +  ^(L)^  -  2yJP(L)J-1) 

+  2vpP(L)'”-1)  -  2vJP(L)J-1)  +  4yJP(L)J-1)  +  oj 
The  off-diagonal  terms  of  the  matrix  block  n(0)^4  are  obtained  as  follows: 

n(0)g  =  n(0)g  =p(L)(;^119) 
n(0)g  =  n(0)g  =P(L)g18 

where 

p mZs  =e(&(d^1)  •  fa(0)<">) 

=E(djc(L)5J”“1) .  (dx(L)("”l)  +  2Sz(L)(n~l)  -  566(L)(n~l) 

=TO£1)  +  2P(L)(”-1}  -  5P(L)(;8-n  ±  2v,P(L)(i;-1)  +  yPP(L)\n-" 
n(0)g  =  n(0)g  =  P(L)g18 
where 

p  =e  •  sm%) 

=E (5y(L)(£X)  •  (dx(L)(”-1}  +  2dz(L)(n_1)  -  5d0(L)("-n  ±  2yp6<p(L)(n~l) 
+  ypS,p(L)(n-]>+t)) 

=P(L)%-P  +  2P(L)(19~31)  -  5P(L)<"9"1)  ±  2>’/,P(L)|9”7)  +  ypP(L)%^ 

n(0)JJ  =  n(0)g  =  p(L)S">19 

where 

P(L)g19  =E(tfy(0)g.^0)g) 

=E|(dj(L)(n-1)  +  6<p(Lin-X)  +  2d<ML)("~1)  ±  ypSziL)^-  +  2y p59(L)(n-'} 
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+  y2p8(p{Lfn~X)  +  rj) .  (6x(L)(n~l)  +  2 8z(L){n~l)  -  560(L)(n-])  ±  2ypdcf>(L)(n-l) 

=P(L)\n~]>  +  2P(L)J“,)  -  5P(L}%1)  +  P(L)\n~])  +  2P(L)<f7_1)  -  5P(L)^“I)  +  2P(L)("9_1) 

+  4P(L)J-1)  -  10P(L)^r)  ±  ypP(L)[n-l>  +  2ypP(L)\n-'>  +  fpP(L)%V)  ±  2ypP(L)%l) 

+  4ypP(L)J-1)  +  2yJP(L)J?-1)  +  5^P(L)J-1)  ±  lOy^L)^  -  5v;P(L)^1} 

±  2ypP{Lt~l)  ±  2ypP(Lf;>-')  ±  4^P(L)^1)  +  2>4,P(L)^1)  -  4^P(L)^1) 

±  2>’pP(L)7”” l}  +  ypP(L)%l)  +  ypP(Lf;-1}  +  2ypP(L)<9n~')  -  y2pP +  2y2pP(L)%l) 

+  yJP(L)^1) 

n(0)(”J  =  n(0)JJ  =  P(L)g19 

where 

P(L)g19  =E(^(0)g  .  8*1)%*) 

=E^(6y(L)(n-])  +  50(L)(,!_1)  +  2St//(L){n~1)  ±  ^(L)^  +  2yp&9(L)("-1) 

+  ^(L)('i-1)  +  77).^(4"2-1)) 

=pwiS) + toS? + 2P(4:r81)  ±  + 2Jpp(l)£-i) + yjp^y 

n(0)g  =  n(0)g  =  P(L)g19 

where 

P(L)Z9  =E(dy(0)^.6y(L)%1))j 

=E^(6y(L)(n-])  +  6<p(L)(n-x)  +  2SiJ/(L){n~1)  +  yp6z(L)(n~r)  +  2 yp66(L)(n~l)  +  y2pS<p(L)(n-]) 
+  Ti).6y(L)%1)}±ypP(L)%? 

=P(L)tu  +  P(L)tu  +  2P(L)9a“91)  +  2>’pP(L)J;-9I)  +  >2P(L)^9r) 

The  state  estimate  transition  for  the  fourth  to  the  n/7z  epochs  is  accomplished  by 


augmenting  the  first  fifteen  states  of  the  previous  epoch  with  the  farthest  ground  feature 
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and  zeros  as  follows 


0 5x+(L))(n+l)  = 


(&t+(L)15xl)<"> 

(Sx+(L)n)(n> 

(&t+(L)19)(n) 


(3.63) 


Starting  at  epoch  4,  the  transitions  for  the  remainder  of  the  epochs  followed  Eqs.  (3.62) 
and  (3.63)  for  epoch  3,  because  there  are  no  more  known  ground  objects. 


3.4.3  Summary.  In  summary,  the  KF  is  reinitialized  at  the  beginning  of  each 
measurement  epoch.  The  KF  operates  twice  in  each  measurement  epoch  at  a  frequency  of 
0.2  [Hz]  in  10  seconds.  At  the  beginning  of  measurement  epoch  one,  the  exact  state  of  the 
aircraft  is  known  with  very  small  uncertainties.  The  location  of  the  two  tracked  ground 
features  are  known  so  the  KF  starts  operation  using  the  fifteen  original  states. 

At  the  beginning  of  measurement  epoch  two,  the  first  ground  feature  is  dropped  out 
of  the  FOV  of  the  camera  and  the  camera  geolocates  the  first  unknown  ground  feature, 
which  becomes  the  second  ground  feature  for  the  second  measurement  epoch.  The  INS 
provided  state  of  the  aircraft  at  the  end  of  measurement  epoch  one  is  used  to  estimate  the 
position  of  the  newly  acquired  unknown  ground  feature.  SLAM  is  achieved  through  the 
augmentation  of  the  fifteen  states  state  vector  by  two  states  (x  and  y  position  of  the  new 
ground  feature)  to  a  seventeen  states  state  vector.  Zeros  are  used  for  the  augmentation 
because  the  derivative  of  position  is  zero. 

In  measurement  epoch  three  and  beyond,  the  errors  in  the  position  of  the  second 
unknown  ground  feature  for  the  previous  measurement  epoch  becomes  the  error  in 
position  of  the  first  unknown  ground  feature.  The  INS  provided  state  of  the  aircraft  at  the 
end  of  the  previous  measurement  epoch  is  used  to  estimate  the  position  of  the  newly 
acquired  unknown  ground  feature.  Because  two  unknown  ground  features  are  used  from 
measurement  epoch  three  and  beyond,  the  KF  operates  with  19  states,  which  includes  15 
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original  states  and  two  states  (x  and  y  position  of  unknown  ground  feature)  each  from  the 
two  unknown  ground  features.  SLAM  is  achieved  through  the  augmentation  of  states  at 
the  beginning  of  each  of  these  measurement  epochs.  Zeros  are  used  for  these 
augmentations  because  the  derivative  of  position  of  the  newly  acquired  unknown  ground 
feature  is  zero. 
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4  Simulation  Results 


4.1  Introduction 

This  chapter  shows  simulation  experiments  that  were  run  for  the  one  hour  duration. 
The  chapter  will  start  with  plots  showing  the  first  three  epochs  of  the  first  three  navigation 
states  for  the  INS.  This  information  will  then  be  followed  by  plots  of  the  first  three  epochs 
of  the  aided  INS.  The  effect  of  tracked  ground  features  which  are  staggered  is  also 
discussed.  Plots  showing  the  remainder  of  the  navigation  states,  for  both  the  unaided  and 
aided  INS  schemes  are  found  in  Appendix  A. 

4.2  Simulation 

Simulation  experiments  were  run  to  gauge  the  strength  of  the  aiding  action  afforded 
by  bearings  only  measurements  using  “bootstrapping”  for  cross  country  flight.  The  aim  is 
to  reduce  the  errors  of  the  free  INS  as  much  as  possible:  the  closer  the  KF-estimated  and 
the  true  errors  are,  the  better  the  aiding  action  is.  To  study  the  effects  of  using  known  and 
unknown  ground  features,  the  first  three  navigation  states  (position)  estimation  errors  and 
standard  deviations  of  the  unaided  (free)  INS  for  the  first  three  epochs  are  plotted  in 
Figure  4.1.  The  error  in  the  x  position  is  of  most  concern  and  its  uncertainty  level  after  the 
duration  of  thirty  seconds  of  flight  is  about  110  [cm],  with  a  realized  error  in  position  of 
about  99  [cm].  For  the  same  duration,  the  standard  deviation  of  the  aided  INS  and  the 
difference  in  the  error  of  the  true  and  estimated  x  position  are  plotted  in  Figure  4.2.  The 
uncertainty  in  the  estimated  x  position  (prediction  of  what  the  difference  between  the  true 
and  estimated  errors  will  be)  after  the  third  epoch  is  about  25  [cm]  (better  than  the  free 
INS),  with  a  realized  error  in  position  of  about  27  [cm]  (better  than  the  free  INS).  It  can  be 
seen  that  with  aiding,  when  the  aircraft’s  position  is  estimated  by  two  known  ground 
features,  the  errors  in  the  position  are  almost  negligible  for  the  first  epoch  (first  10 
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seconds).  For  the  second  epoch  (next  10  seconds),  when  aiding  is  achieved  by  using  one 
known  and  one  unknown  geolocated  ground  feature,  the  errors  start  increasing  because  of 
the  higher  uncertainty  added  by  the  unknown  feature’s  geolocation  on  the  fly.  With  the 
lost  of  the  last  known  ground  feature  and  addition  of  another  unknown  feature,  the 
uncertainty  is  even  higher  with  the  difference  in  the  true  and  estimated  error  in  position 
eventually  falling  outside  the  aided  INS  predicted  standard  deviation. 

Next,  the  simulation  results  for  the  whole  one  hour  flight  for  the  free  and  aided  INS 
are  respectively  plotted  in  Figures  A.l  and  A. 2.  The  uncertainty  in  the  x  position  for  the 
free  INS  after  one  hour  is  about  six  and  a  half  kilometers,  with  a  realized  error  in  position 
of  about  6.02  [km]  in  Figure  A.l.  It  can  be  seen  from  the  plots  that  with  aiding  using  the  9 
mega  pixel  camera,  the  uncertainty  in  the  x  position  is  significantly  reduced  in  Figure  A. 2. 
After  a  one  hour  duration,  the  uncertainty  in  x  position  is  only  about  693  [in]  with  a 
realized  error  in  position  of  about  588  [m\.  It  is  worth  noting  that  from  the  third  to  the  last 
epoch,  even  with  the  introduction  of  two  unknown  ground  features  as  opposed  to  at  least 
one  known  ground  feature,  the  Kalman  Filter  learns  and  eventually  reduce  the  estimation 
errors  in  the  x  position. 

In  the  scenario  where  ground  features  are  arranged  in  a  straight  line,  though  aiding 
was  primarily  achieved  in  the  x  position,  the  uncertainty  and  realized  error  in  the  y 
position  are  also  significantly  reduced  from  six  and  a  half  kilometers  and  5.98  [km]  to 
about  18.84  [m]  and  1.78  [m]  respectively.  When  the  ground  features  are  laterally 
staggered  10  [m]  about  the  aircraft  trajectory,  Figure  A. 3,  the  uncertainty  in  the  x  and  y 
positions  remains  almost  the  same.  Beyond  10  [m]  displacement,  the  KF  is  unstable.  So, 
as  long  as  the  ground  features  are  within  the  LOS  of  the  camera,  aiding  in  the  x  direction 
is  not  impacted,  and  there  is  little  to  no  impact  in  the  y  direction. 

There  also  are  improvements  in  the  other  seven  navigation  states  estimates,  as  shown 
in  Figures  A. 2  through  A. 5.  In  Figures  A.4  and  A. 5,  the  plots  show  how  the  KF  closely 
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Free  INS  for  First  Four  Epochs 


0  5  10  15  20  25  30  35 

Time  (sec) 

Figure  4.1:  The  development  of  the  KF  predicted  standard  deviation  and  realized  position 
estimates  of  the  unaided  INS  in  the  first  three  measurement  epochs. 


estimate  the  true  error  (again  the  aim  is  to  closely  track  the  true  errors  to  eliminate  their 
effect  in  the  navigation  process),  thus  achieving  aiding  action.  This  information  is 
potrayed  in  Table  4.1. 
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Aided  INS  for  First  Four  Epochs 


x  10'3 


Figure  4.2:  The  development  of  the  KF  predicted  standard  deviation  and  realized  position 
estimates  of  the  aided  INS  for  the  first  three  measurement  epochs. 


The  calculated  first  ground  feature  and  geolocation  of  second  ground  feature  are 
shown  in  Figure  A. 7.  The  x  position  of  the  first  ground  feature  starts  at  one  kilometer 
(because  its  position  was  exactly  known)  and  it  is  calculated  up  to  360.61  [km]  at  the  end 
of  one  hour.  Its  y  position  starts  at  zero  and  it  is  calculated  up  to  about  four  and  a  half 
meters  at  the  end  of  one  hour.  Likewise,  the  x  position  of  the  second  ground  feature  starts 
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Table  4.1:  The  final  values  for  the  standard  deviations  and  errors  for 
the  unaided  navigation  states.  Also  included  are  the  final  values  for 
the  standard  deviations  and  errors  for  the  aided  navigation  states  in 
both  the  scenario  when  the  ground  features  are  in  a  straight  line  and 
when  they  are  staggered. 


Standard 

Deviation/Error 

Unaided  Final 
Value 

Aided  Final 
Value(Finear) 

Aided  Final 
Value(Staggered) 

crx  ( s.d ) 

6.56  [km] 

692.72  [m] 

692.70  [m] 

8x  {err) 

6.02  [km] 

587.84  [m] 

697.19  [m] 

cry  {s.d) 

6.56  [km] 

18.84  [m] 

18.88  [m] 

8y  {err) 

5.98  [km] 

1.78  [m] 

2.54  [m] 

crz  {s.d) 

707.11  [m] 

5.72  [m] 

5.72  [m] 

Sz  {err) 

18.96  [m] 

5.04  [m] 

5.60  [m] 

crVx  {s.d) 

3.67  x  10~2  [m/s] 

5.74  x  10~3  [m/s] 

5.74  x  10-3  [m/s] 

8vx  {err) 

3.21  x  10~2  [m/s] 

5.15  x  10~3  [m/s] 

5.53  x  10~3  [m/s] 

CTyy  {S.d) 

3.67  x  10-2  [m/s] 

8.66  x  10-5  [m/s] 

8.70  x  10~5  [m/s] 

8vy  {err) 

3.19  x  10~2  [m/s] 

4.54  x  10~5  [m/s] 

2.52  x  10~6  [m/s] 

crVz  {s.d) 

3.93  x  10~3  [m/s] 

3.18  x  10~5  [m/s] 

3.18  x  10-5  [m/s] 

Svz  {err) 

1.02  x  10“4  [m/s] 

2.86  x  10~5  [m/s] 

3.06  x  10~5  [m/s] 

(s.d) 

1.05  x  10-4  [rad] 

1.09  x  10-5  [rad] 

1.09  x  10-5  [rad] 

8(f)  {err) 

7.82  x  10~5  [rad] 

1.10  x  10~6  [rad] 

1.38  x  10~6  [rad] 

cre  {s.d) 

1.05  x  10“4  [rad] 

3.27  x  10-5  [rad] 

3.27  x  10-5  [rad] 

86  {err) 

7.82  x  10~5  [rad] 

3.30  x  10-5  [rad] 

2.94  x  10-5  [rad] 

o>  (s.d) 

1.05  x  10“4  [rad] 

5.85  x  10-5  [rad] 

5.87  x  10-5  [rad] 

8ip  (err) 

7.82  x  10~5  [rad] 

1.02  x  10~5  [rad] 

1.66  x  10-5  [rad] 

at  two  kilometers  (because  its  position  was  exactly  known)  and  it  is  calculated  up  to 
361.60  [km]  at  the  end  of  one  hour.  Its  y  position  starts  at  zero  and  it  is  calculated  up  to 
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about  five  and  a  fifth  meters  at  the  end  of  one  hour.  Figures  A. 8  and  A. 9  shows  the 
uncertainties  of  the  first  eight  epochs  of  the  first  and  second  ground  features.  Each  epoch 
consists  of  2  bearing  measurements  which  are  sampled  at  a  rate  of  0.2  [Hz].  Each  spike 
corresponds  to  the  utilization  of  bearing  measurements.  During  a  measurement  epoch  the 
error  increases.  At  the  beginning  of  a  new  measurement  epoch  the  knowledge  from  the 
previous  epochs  causes  the  errors  to  decrease. 

The  errors  in  the  positions  of  the  aircraft  with  the  attendant  uncertainties  for  the  aided 
INS,  and  the  two  calculated  ground  features,  are  shown  in  Figure  A.  10.  This  shows  the 
accuracy  in  the  calculated  ground  features  and  how  well  they  are  used  to  estimate  errors  in 
the  aircraft  navigation  process. 
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5  Conclusion 


In  [12]  covariance  anlysis  was  performed  and  it  was  shown  that  the  rate  of  growth  of 
position  uncertainty  is  significantly  reduced  when  the  aircraft  continuously  uses  the 
measurement  of  the  bearings  of  sequentially  acquired  unknown  terrain  features  to  aid  the 
INS.  In  this  paper  the  analysis  is  refined.  The  LOS  errors  that  will  be  present  when  using 
an  optical  camera  to  aid  the  INS  are  accounted  for.  Furthermore,  in  this  paper  the  Kalman 
Filter’s  design  is  provided  and  its  performance  is  validated  by  simulation  of  the  KF  action. 
The  on-board  INS  of  an  aircraft  was  aided  by  an  optical  camera  used  to  take  the  bearing 
measurements  of  ground  features.  So  long  as  the  ground  features  are  regularly  spaced  and 
are  not  more  laterally  displaced  than  10  [m\  about  the  aircraft’s  trajectory,  the  LOS  of  the 
camera,  the  INS  aiding  action  is  strong.  It  is  a  cyclic  process  where  the  aircraft  uses  its 
INS-provided  ownship  position  to  geolocate  a  ground  object,  then  uses  the  bearing 
measurements  of  the  very  same  ground  feature  to  aid  its  INS. 

The  navigation  state  of  most  concern  was  the  aircraft’s  position.  For  a  one  hour 
flight,  it  was  shown  that  with  INS  aiding,  the  error  in  the  aided  INS  navigation  system 
provided  position  of  the  aircraft  was  significantly  reduced  from  about  3.87  [km]  with  a  KF 
predicted  uncertainty  of  about  6.5  [km],  to  about  583  [m]  with  an  uncertainty  of  693  [/»]. 
This  validates  our  analysis  and  showed  that  INS  aiding  using  vision  is  possible  for  long 
range  flight.  The  practicality  of  this  INS  aiding  scheme  hinges  on  the  robustness  of  the 
image  registration  scheme,  in  particular,  in  an  outdoors  setting.  In  [5],  [6],  and  [7]  these 
issues  are  addressed  using  the  Scale-Invariant  Feature  Transform  (SIFT)  algorithms  to 
detect  images  registered  on  an  optical  camera’s  focal  plane. 
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Appendix  A:  Simulation  Results 
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Figure  A.l:  The  development  of  the  KF  predicted  standard  deviation  and  realized  position 
estimate  of  the  aided  INS  during  a  one  hour  flight. 
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Aided  INS  for  One  Hour  for  Ground  Features-Linear 


x  10"3 


Figure  A. 2:  The  development  of  the  KF  predicted  standard  deviation  and  realized  position 
estimates  of  the  aided  INS  during  a  one  hour  flight  for  ground  features  arranged  in  a  straight 
line. 
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Aided  INS  for  One  Hour  for  Ground  Features-Staggered 


x  10"3 


Figure  A. 3:  The  development  of  the  KF  predicted  standard  deviation  and  realized  position 
estimates  in  the  aided  INS  during  a  one  hour  flight  for  staggered  ground  features. 
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True  &  Estimated  Velocity  Errors 


x  10 


flight. 
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True  &  Estimated  Attitude  Errors 
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flight. 
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Geolocated  Ground  Features  Positions  from  Start  of  Flight 


x  10“3 


x  10“3 
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Ground  Feature  1  Position  Uncertainty 


x  10-4 


Figure  A. 8:  A  zoomed  in  view  of  the  development  of  the  KF  predicted  standard  deviation 
of  the  first  ground  feature’s  position  in  first  seven  measurement  epochs. 


Ground  Feature  2  Position  Uncertainty 


x  ICf4 


Figure  A. 9:  A  zoomed  in  view  of  the  development  of  the  KF  predicted  standard  deviation 
of  the  second  ground  object’s  position  in  first  seven  measurement  epochs. 
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Aircraft  and  Ground  Features  Position  and  Uncertainty  in  X 


Figure  A.  10:  Aircraft  and  ground  objects’  position  estimates  with  KF  predicted  standard 
deviations. 


72 


Appendix  B:  Ground  Feature  Calculations 


The  calculation  and  geolocation  of  ground  features  used  for  aiding  are  shown  in 
Tables  B.l  -  B.3  below: 


Table  B.l:  Ground  Features  on  X-Axis:  X  Position 


Epoch  # 

Xplc 

Xp2c 

1 

1 

2 

2 

2 

3  +6x(L) 

n{>  3) 

n  +  Sx(L(n  -  2)) 

77  +  1  +  Sx(L(n  -  1)) 

Table  B.2:  Ground  Features  on  X-Axis:  Y  Position 


Epoch  # 

yPic 

yP2c 

1 

0 

0 

2 

0 

Sy(L) 

n(>  3) 

Sy(L(n  -  2)) 

8y{L(n  -  1)) 

Table  B.3:  Y  Positions  of  Laterally  Staggered  Ground  Features  on  Both  Sides  of  X-Axis 


Epoch  # 

yPic 

yP2c 

1 

-yP 

yP 

2 

yP 

-yp  +  Sy(L) 

77(>  3) 

~yP  +  8y(L(n  -  2)) 

yp  +  6y(L(n  -  1)) 

where  n  is  the  epoch  number. 
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